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SECTION  1 


INTRODUCTION 


1.1  Purpose 

This  volume  provides  a detailed  description  of  the  simulator  pro- 
gram from  a functional,  structural,  and  operational  point  of  view. 

The  primary  purpose  of  the  program  is  tc  permit  the  evaluation  of 
the  navigation  and  attitude  errors  resulting  from  the  mechanization  of 
a strapdown  inertial  navigation  system,  using  detailed  sensor  models, 
in  a highly  dynamic  tactical  aircraft  environment.  This  purpose  should 
be  complementary  to  that  of  another  CSDL-deve loped  simulation  (R-977) , 
which  assumed  “perfect”  sensors,  and  was  directed  towards  the  evaluation 
of  the  computational  errors  resulting  from  the  mechanization  of  the 
navigation  and  attitude  equations  in  a digital  computer.  During  the 
latter  part  of  the  present  task  the  simulated  navigation  computer  soft- 
ware was  modified  to  conform  with  the  “upgraded"  version  of  the  algorithms 
employed  in  the  reference  insofar  as  was  possible  without  changing  the 
structure  of  the  program. 

The  equations  developed  or  presented  in  analytical  form  in  Volume  IX, 
appear  in  this  volume  in  program  mnemonics  in  "Fortranesque"  form. 

1.2  Program  Function 

Although  the  program  consists  of  a number  of  quasi-independent, 
replaceable  modules,  as  will  bo  described  later,  it  may  be  more  easily 
understood  in  terms  of  the  four  major  functions  performed. 


The  first  function  is  the  aircraft  profile  or  trajectory  genera- 
tion. The  intended  source  of  the  gross  vehicle  dynamics  (position,  £) 

velocity,  vj  attitude,  specific  force,  a^  angular  velocity  and 
b * b 

acceleration,  10  and  u ) is  the  AFAL  profile  generator  program, 

PROFGEN.  For  test  and  checkout,  the  present  version  of  the  INSS 
employs  a simplified,  self-contained,  non  maneuvering,  "gross"  profile 
generator.  The  linear  and  angular  vibration  environment  is  generated 
in  another  module  and  superimposed  on  the  gross  trajectory. 

The  profile  generator  provides  both  a reference  profile  and  the 
forcing  functions  which  drive  the  simulated  sensors. 

The  second  function  is  the  simulation  of  the  dynamics  of  the 
inertial  measurement  unit  (IM1?)  and  the  barometric  altimeter.  Three 
single  degree  of  freedom,  floated,  rate  integrating,  wheel-type  gyros 
or  three  ring  laser  gyros  may  be  simulated,  plus  three,  single-axis 
pendulous  accelerometers.  Specific  forco  and  angular  velocity  from 
the  profile  or  trajectory  are  the  primary  forcing  functions  for  the 
inertial  components,  while  the  models  are  defined  by  the  input  para- 
meters for  the  sensors. 

The  I MU  simulator  provides  the  incremental  angles,  AO/*,  and  incre- 
mental velocities,  AVa.  and  the  indicated  barometric  altitude,  hQ,  to 
the  navigation  computer  interface  unit,  just  as  if  they  wore  obtained 
from  actual  sensors. 

The  third  function  is  the  simulation  of  a navigation  (and  attitude) 
computer  for  a sstrapdowii  aircraft  inertial  > ^vigation  system  (IMS).  The 
first  major  subtask  of  the  navigation  computer  is  to  correct  the  Ava,s 
and  AQ^*s,  using  the  loaded  values  of  the  component  parameters  and  the 
compensation  models,  so  that  the  resultant  computed  values,  AV^  and 
Aft^.  approach,  the  integrals  of  the  specific  force  and  angular  rate, 
output  by  the  profile  generator. 
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The  second  major  subtask  of  the  navigation  computer  is  the  updating 

X Is 

of  the  body  to  inertial  transformation,  C^,  using  the  AO^'s,  and  the 
subsequent  transformation  of  the  AV^'s  from  the  body  to  the  inertial 
frame. 

The  final  major  subtask  of  the  navigation  computer  is  the  updating 
of  the  position,  and  velocity,  V , and  the  extraction  of  the  attitude 
angles,  in  the  navigation  and  attitude  algorithms. 

The  fourth  function  is  the  evaluation  of  the  errors  produced  by 
the  second  and  third  functions  relative  to  the  reference  values 
generated  by  the  profile. 

The  evaluation  module  outputs  both  the  instantaneous,  reference 
values  of  position,  velocity,  and  attitude  and  the  "errors"  in  the 
computed  values  of  the  same. 

This  gross  functional  breakdown  of  the  INS5  is  illustrated  in 
Figure  1-1. 

1 . 3 Overview  of  Functional  Structure 

The  INSS  program  consists  of  a number  of  modules,  each  of  which 
performs  part  or  all  of  one  of  the  four  major  functions  described 
in  the  preceding  subsection. 

Each  module  contains  its  own  initialisation  data  (XFX1£)  which 
includes  both  module  parameter  and  timing  and  control.  In  addition, 
each  has  access  to  a physical  data  file  (POATA) . which  provides  some 
physical  constants  and  the  initial  conditions  for  the  simulated  flight. 

There  really  is  no  executive  for  the  INSS  program  in  the  normally 
understood  sense,  only  a sequencer,  which  calls  the  modules  in  order 
and,  in  some  canes,  buffers  some  of  the  module  input/output  data, 
lienee  in  expanding  the  description  of  the  program,  the  sequencer  docs 
not  enter  into  the  picture  directly,  beyond  establishing  the  order  an 
which  the  modules  are  called. 
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In  terms  of  the  individual  modules  the  INSS  program  performs  the 

following  functions: 

1.  Provides  a flight  profile  for  a point-mass  aircraft  executing 
coordinated  maneuvers.  Profile  includes  position  velocity, 
acceleration  and  attitude  of  the  aircraft. 

2.  Superimposes  on  the  flight  profile,  the  angular  and  linear  motion 
of  the  aircraft  in  response  to  random  aerodynamic  forces  such  as 
gusts  and  turbulence. 

3.  Operates  on  the  specific  force  and  angular  velocity  to  produce 

the  ideal  body  frame  values  of  these  quantities  and  the  correspond- 
ing angular  acceleration. 

4.  Integrates  the  gyro  and  accelerometer  equations  of  motion,  of  the 
form  specified,  after  accounting  for  the  displacements  of  the 
inertial  components  from  the  "center"  of  the  point-mass  vehicle, 

and  their  orientations  with  respect  to  the  body  frame.  Incorporates 
effects  of  component  parameters  and  environment  on  component  out- 
puts. 

5.  Reads  true  altitude  and  perturbs  the  same  according  to  the  alti- 
meter model. 

6.  Transmits  sensor  data  from  simulated  IMU  to  simulated  navigation 
computer,  and  resets  sums. 

7.  Performs  accelerometer  compensation  function  (this  is  essentially 
the  inverse  of  the  accelerometer  model  - with  errors  and  omissions) . 

8.  Using  compensated  accelerometer  outputs  (AV's)  and  gyro  outputs 
(AO's)  performs  the  gyro  compensation  (again,  this  is  essentially 
the  inverse  of  the  gyro  model  - with  errors  and  omissions) . 

9.  Updates  the  body  to  inertial  frame  transformation  (direction  cosine 
matrix,  d.c.m.)  and  transforms  the  incremental  velocities  to  the 
inertial  frame  (either  a d.c.m.  or  a quaternion  update  may  be 
employed) . 
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10.  Transforms  the  incremental  velocities  to  the  local  vertical  wander 
azimuth  computational  frame  and  computes  local  vertical  position, 
velocity,  and  attitude  (incorporating  the  barometric  altimeter  for 
vertical  damping) . 

11.  As  required,  the  navigator  outputs  are  differenced  with  the  flight 
profile  values  and  the  resulting  errors  are  printed  out  and/or 
plotted. 

The  foregoing  description  of  the  program  flow  is  presented  pictor- 
ially  in  Figure  1-2,  where  the  dashed  lines  indicate  the  major  functions 
shown  in  Figure  1-1. 


Figure  1-2  INSS  Functional  Block  Diagram 


SECTION  2 


PROGRAM  DESCRIPTION 


2 . 1 Introduction 

The  INSS  simulator  consists  of  11  subprograms  (modules)  which  simulate 
the  various  hardware  and  software  functions  in  a strapdown  system.  A 
sequencer  (main)  program  interrogates  each  module  in  turn  at  one  of  two 
different  user-specified  frequencies.  Associated  with  each  module  are 
initialization  data  files  that  also  contain  user-specified  switches  and 
timing  specifications  so  that  each  module  interrogated  by  the  sequencer 
can  control  its  own  operating  time,  data  output  frequencies,  initial-  and 
last-pass  program  functions.  Each  module  initialization  data  file  (IFILE) 
has  a unique  input/output  (I/O)  file  unit  number  which  is  identified  with 
each  module.  Also,  every  module  has  access  to  a common  data  file  (PDATA) 
that  contains  physical  data  and  initial-tra jectory  flight-path  parameters 
required  to  initialize  certain  modules. 

In  Table  1 are  listed  the  INSS  program  modules.  The  default  initializa- 
tion data  files  are  listed  by  name  and  FORTRAN  unit  number  in  Table  2.  Two 
replacement  modules  are  listed  with  their  default  data  file  names  in  Table  3. 

In  addition  there  is  a set  of  library  subroutines  for  the  purpose  of 
performing  repeated  common  calculations. 

o Matrix  transpose  by  matrix  multiplication  (MTXM) 
o Matrix  transpose  by  vector  multiplication  (MTXV) 
o Matrix  by  matrix  multiplication  (MXM) 
o Matrix  by  vector  multiplication  (MXV) 
o Gaussian  random  number  generator  with  specifiable 

t 

mean  and  standard  deviation  (GAUSS) 
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Table  1.  INSS  Programs 


Program  j 

Dal  aunt 

Huh*  mil  1 no 

V'hiihMmi* 

INSSEQ.FORT 

Main 

Executive 

INSTRJ.FORT 

TRJ 

Interim  Trajectory  Data-Processor/ 
Interface  Module 

INSENV.FCRT 

ENV 

Hardware  Vibration  Simulator  Module 

INSGYR.FORT 

GYROS 

Hardware  Simulator  Gyro  Module 

I NS ACC. FORT 

ACCEL 

Hardware  Simulator  Accelerometer  Module 

INSALT. FORT 

ALTI 

Hardware  Simulator  Altimeter  Module 

INSRDR. FORT 

RDR 

Hardware/Software  Interface  Module 

INSCAC • FORT 

ACOMP 

Software  Accelerometer-Compensation  Module 

INSCGY.FORT 

GCOMP 

Software  Gyro-Compensation  Module 

INSALG.FORT 

ALG 

Software  Velocity  Attitude  Module 

INSLLN.FORT 

LLN 

Software  Navigation  Module 

INSEVL.FORT 

EVL 

j Evaluation  Module 

Table  2.  Data  Files 


Initialization 

Data  Base 

I/O  File 

Unit  No. 

Initialization 

Data  Base 

I/O  File 

Unit  No. 

SEQ.DATA 

10 

RDRD . DATA 

65 

TRJD . DATA 

20 

CACD.DATA 

67 

ENVD . DATA 

30 

CGYD . DATA 

69 

GYRD.DATA 

40 

ALGD . DATA 

70 

ACCD.DATA 

50 

LLND.DATA 

80 

ALTD . DATA 

60 

E VLD . DATA 

90 

PHYSD.DATA 

7 
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Table  3.  Replacement  modules  and  corresponding  data  sets. 


Program 

Dataset 

Sub- 

routine 

Initializa- 
tion Dataset 

I/O  File 
Unit  No. 

Function 

INSGYRL.FORT 

GYROS 

GYRLD . DATA 

40 

Hardware  Simulator 
Laser-Gyro  Module 

INSCGYL . FORT 

GCOMP 

CGYLD.DATA 

69 

Software  Laser-Gyro 
Compensation  Module 

There  is  also  a set  of  subroutines  that  are  required  solely  by  the 
local  level  navigator  module  (LLN) . These  subroutines  have  the  following 
functions, 

o Angular  velocity  calculation  (ANGVEL) 

o Angular  velocity  by  DT  and  Coriolis  correction  (TORCOR) 

o DCM  second  order  update  matrix  (AUP) 

o Gravity  computation  (GRAV) 

o 3x3  matrix  multiply  (MM) 

The  INSS  simulator  has  the  operational  flexibility  to  allow  for  the 
replacement  of  modules  having  the  same  function  but  different  formulation. 
The  initialization  files  for  each  of  the  modules  may  be  readily  replaced 
in  total  or  particular  data  elements  within  the  data  file  may  be  replaced. 
Printed  output  frequency  may  be  controlled  by  the  selection  of  a printing 
interval  variable  in  the  initialization  file  of  the  modules. 

2.2  Operation 

(1)  Module  Replacement 

In  order  to  replace  a module  having  the  same  function  but  different 
formulation,  it  is  only  necessary  to  retain  the  same  subroutine  name  and 
argument  list.  Module  replacement  is  accomplished  merely  by  physically 
substituting  the  appropriate  module  deck  in  a batch  process  computer  run 
or  by  editing  the  program  files  in  the  computer  storage.  The  actual  re- 
placement method  is  peculiar  to  the  computer  setup  available. 

(2)  Data  File  Replacement 

Each  of  the  modules  has  an  associated  initialization  data  file. 
Again,  for  convenience,  the  whole  data  file  may  be  replaced  by  a new  file 
of  data  values.  This  operation  may  be  the  physical  replacement  of  data 
deck  in  a batch  process  or  substitution  of  a data  file  from  a disk  or 
tape  storage.  A new  data  file  usually  accompanies  the  substitution  of 
a new  program  module. 
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(3)  Data  Value  Override 

Within  any  particular  initialization  file,  it  is  often  necessary  to  use 
a different  data  value  for  some  variable.  To  accomplish  this  overriding  for 
each  variable,  it  is  only  necessary  to  append  a new  record  at  the  end  of  the 
initialization  file  to  be  changed.  This  record  requires  an  (15,  F20.10) 
format  for  reading  an  index  number,  IX,  and  data  value,  DATA(IX) . The  index 
IX,  referes  to  the  variable  number  in  the  file,  for  which  an  override  is 
desired  and  the  variable  DATA (IX)  contains  the  value  of  the  variable.  Any 
number  of  overrides  may  be  appended  for  the  same  or  different  variables 
within  each  of  the  initialization  files,  since  only  the  latest  value  is  retained. 

(4)  Simulation  Termination 

A simulation  is  normally  terminated  by  the  user  specifying  the  termination 
time,  TEND,  in  the  sequencer  (SEO)  initialization  file.  When  the  simulation 
time  reaches  the  value  TEND,  a last  pass  indicator,  IENDF , is  set  to  1,  so 
that  each  module  will  receive  a last  pass  to  terminate  normally. 

(5)  Data  Output  and  Print  Control 

There  are  several  modes  for  outputting  data  from  the  simulator.  From 
each  module,  data  may  be  output  at  a user  specified  frequencv  as  described 
in  the  flow  chart,  Figure  2-1.  The  evaluation  module  (EVL)  supplies  the  basic 
output  of  the  simulator  and  an  identical  data  file  to  the  evaluation  module 
print  file  may  be  stored  on  disc  or  tape. 
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To  provide  sufficient  flexibility  in  simulator-data  output  and 
module  print  control,  three  user-specified  data  items  are  provided  to 

(1)  Allow  general  data  output  of  all  modules  at  the  same  frequency, 

PRNTDT  (s)  This  data  item  must  be  specified  in  the  INSS 
common  data  file  (PDATA) . This  item  provides 
a general  data-output  print  frequency  for  all 
modules  and  overrides  any  values  specified  in 
MODPDT  in  each  module 

If  PRNTDT-=0,  module  printing  occurs  only  if 
M0DPDT>0  and  PRNTSW=1.  Printing  will  then  occur 
at  the  frequency  specified  in  MODPDT  of  each 
module.  If  MODPDT=0,  no  module  printing  occurs. 

If  PRNTDTK),  module  printing  occurs  for  all 
modules  at  the  frequency  specified  in  PRNTDT  if 
PRNTSW= 1 regardless  of  the  values  of  MODPDT  for 
each  module. 

(2)  Allow  individual  module  printing  at  different  frequencies. 

MODPDT  (s)  This  data  item  must  be  specified  in  the 

initialization  data  file  of  each  module.  This 
item  is  used  to  control  thQ  frequency  of  module 
output  data  printing  and  provides  capability 
for  different  modules  to  print  at  different 
frequencies. 

If  M0DPDT“0 , this  state  is  tested  only  if 
PRNTDT=0 , and  then  no  module  printing  occurs. 

If  M0DPDT>0,  module  printing  occurs  at  the 
frequency  specified  in  MODPDT,  provided  pRNTDT«0 
and  PRNTSW-1. 
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(3)  provide  an  individual  module-printing  on/off  switch  to  control 
printing  in  each  module. 

PRNTSW  This  data  item  must  be  specified  in  the 

initialization  data  file  of  each  module  and  is 
simply  an  on/off  switch. 

If  PRNTSW=0,  no  module  printing  occurs  regardless 
of  the  contents  of  MODPDT  and  PRNTDT. 

If  PRNTSW>1,  module  output  data  printing  occurs 
provided  both  MODPDT  and  PRNTDT  are  not  0, 

The  evaluation-module  (EVL)  data  output  is  in  tables  containing 
trajectory  attitude,  position,  velocity,  and  the  corresponding  navigation 
errors  which  are  printed  online  every  50  EVL-module  operating  cycles  if 
PRNTSW=1.  The  user  controls  EVL-module  operating  frequency  in  the  usual 
way  by  specifying  a value  for  DT  in  the  initialization  data  file.  See 
Fig.  1 for  Flow  Chart,  A tape  or  disc  file  may  be  written  on  FORTRAN  unit 
12  with  the  same  format  and  at  the  same  frequency  as  the  printed  output  file. 

(6)  Memory  Storage  Requirement 

The  total  storage  required  for  the  INSS  system  on  an  Amdahl  computer 
is  approximately  125k  bytes. 
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Figuro  2,1  Data  Output  and  Print  Control 
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2.3  INSS  Module  Descriptions 

The  modules  and  subroutines  available  for  use  in  the  INSS  simu- 
lator are  described.  Shown  here  is  an  index  of  the  modules. 


2.3.1  INSS  Sequencer  (SEQ) 

2.3.2  Interim  Trajectory  (TRAJ) 

2.3.3  Environment  (ENV) 

2.3.4  Gyro  (GYROS) 

2.3.5  Laser  Gyro  (GYROS) 

2.3.6  Accelerometer  (ACCEL) 


2.3.7  Altimeter  (ALTI) 

2.3.8  Hardware/Software  Interface  (RDR) 

2.3.9  Accelerometer  Compensation  (ACOMP) 

2.3.10  Gyro  Compensation  (GCOMP) 

2.3.11  Laser  Gyro  Compensation  (GCOMP) 

2.3.12  Attitude  and  Velocity  Algorithm  (AL6) 

2.3.13  Local  Level  Navigator  (LLN) 

• Angular  Velocity  (ANGVEL) 

• Angular  Velocity  by  DT  and  Coriolis  Corrections  (TORCOR) 

• DCM  Second  Order  Update  Matrix  (AUP) 

• Gravity  Computation  (GRAV) 

• Matrix  Multiply  (MM) 

2.3.14  Evaluation  (EVL) 

2.3.15  Mathematical  Subroutines 
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2.3.1  MAIN  PROGRAM  (SEQ)  (INSS  SEQUENCER) 

(1)  General  Description 

The  SEQ  module  is  the  main  program  of  the  INSS.  It  functions  as 
a synchronous  executive  by  calling  the  hardware  subprograms  at  a fast 
frequency  and  the  software  programs  at  a slow  frequency.  The  two 
subroutine  call  frequencies  reflect  the  different  computation  rates 
needed  to  simulate  hardware  and  software.  It  should  be  further  noted 
that  each  subprogram  has  its  own  operating  frequency  at  which  it  per- 
forms its  function.  When  called  by  SEQ,  each  subprogram  performs  its 
function  anew  only  if  its  operating  cycle  time  has  elapsed.  Otherwise 
it  immediately  returns  control  to  SEQ.  In  order  to  synchronise  the 
various  operating  frequencies  the  following  restrictions  apply  to 
the  various  cycle  times?  the  slow  rate  cycle  time  for  calls  to  soft- 
ware subprograms  must  be  an  integer  multiple  of  the  fast  cycle  time  for 
calls  to  the  hardware  subprograms i each  subprogram  operating  cycle  time 
must  be  equal  to  or  a greater  integer  multiple  of  the  cycle  time  at 
which  it  is  called. 

(2)  Sequencer  Module  Flow  Diagram 

The  general  structure  of  the  SEQ  module  logic,  and  the  order  in 
which  all  other  modules  are  invoked  are  shown  in  figure  if  With  regard 
to  this  flow  -hart,  several  points  are  worthy  of  particular  attention. 

The  slow  cycle  time,  which  is  chosen  to  reflect  computation  frequency 
of  tho  software  functions  in  a strapdown  navigator,  is  required  to  be  an 
integer  multiple  of  the  fast  cycle  time  that  indicates  the  sampling  fre- 
quency (or  data  availability)  of  the  trajectory,  vibration,  and  instrument 
measurements,  exclusive  of  the  altimeter  module.  The  PDATA  file  is 
written  on  by  tho  trajectory  (TKJ)  module  only,  and,  with  the  exception 
of  the  evaluation  (EVL)  module,  subsequently  becomes  an  element  common  to 
all  other  modules. 

•Each  module  flow  diagram  is  identified  as  Figure  1 within  the 
module  description  it  pertains  to. 
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Figure  1,  Sequencer  module. 
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(3)  Input 


(ci)  Test  Identification  File 

FORTRAN  unit  number:  5 
FORTRAN  format : 9A8 

This  file  consists  of  one  record  containing  a 72-character  user- 
specified  identification  of  a simulation  run. 


(b)  Program  Initialization  File  (IFILE) 


FORTRAN 

unit  number: 

10 

FORTRAN 

format:  15, 

F20.10 

Index 

Variable 

Default 

Value 

Units 

Description 

1 

DT 

0.01 

s 

overall  module  operating  cycle  time 

2 

PRNTSW 

1.0 

logical 

print  switch  0 - no  print 
otherwise  - print  output 

3 

OUTPSW 

0.0 

logical 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for  printout 

5 

SPAREl 

0.0 

not  used 

6 

SPARE2 

0.0 

not  used 

7 

TEND 

60.0 

s 

simulation  end  time 

8 

DTSLOW 

0.02 

s 

module  slow  operating  cycle  time 

9 

MODPDT 

6.0 

s 

module  print  interval 

(c) 

Common  Initialization 

File  (PFILE) 

FORTRAN 

unit  number: 

7 

FORTRAN 

format:  15, 

F20 

.10 
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Index 

Variable 

Default 

Value 

Units 

Description 

1 

WE 

0 . 72921151470E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20975640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 to  20 

PBUF(l) 

o 

• 

o 

— 

not  used 

(4)  Call-Line  Data 

Call- line  input-output  data  are  not  applicable  to  SEQ. 

(5)  Formulation 

(a)  Initialization 

The  following  functions  are  performed  during  the  first  pass. 

• SIMEND  is  initialized  to  0. 

• Read  and  print  test  identification. 

• Read  and  print  initialization  data  (IFILE) . 

• Read  and  print  common  initialization  data  (PFILE) . 

• Set  OFILE  = XFILE . 

• Initialize  simulation  time  at  T = 0. 

• Each  of  the  program  modules  is  called  in  sequence  as  illustrated 
in  the  General  Functions  Section. 

(b)  General  Functions 

SEQ  is  primarily  a series  of  FORTRAN  CALL  statements  which  call 
each  module  in  turn  at  the  user-specified  sequencer  operating  frequency. 
All  interface  data  are  passed  from  one  module  to  another  by  parameter 
lists  where  dummy  variables  are  used  to  protect  input  data. 
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The  following  is  the  series  of  CALL  statements  sequenced  every 
operating  cycle.  The  parameter  lists  are  shown  with  the  variable  rather 
than  the  dummy  name.  Data  items  on  the  second  line  of  each  statement 
are  module  output. 


CALL  TRAJ 


INPUT 


(T,  IENDF,  PRNTSW,  MODPDT, 


LAT,  LON,  ALT,  VEL,  PITCH,  YAW,  ROLL,  DVT, 
HDING,  WANDER,  AB,  WB) 


OUTPUT 


CALL  ENV 


INPUT 
(T,  IENDF,  AB,  WB, 


ABB,  WBB,  WBBDOT) 
OUTPUT 


CALL  GYROS 


^INPUT^ 

(T,  IENDF,  WBB,  WBBDOT,  ABB, 


DTHETA) 

OUTPUT 


CALL  ACCEL 


INPUT^ 

(T,  IENDF,  ABB,  WBB,  WBBDOT, 


INPUT 
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CALL  ALTI 


INPUT 
(T,  IENDF,  ALT,  VEL, 


ALTO) 

OUTPUT 


CALL  RDR 


INPUT 

(T,  IENDF,  DTHETA,  DV, 


DTHETO,  VDO) 
OUTPUT 


CALL  ACOMP 


INPUT 

(T,  IENDF,  DVO,  DTHETO, 


DVA) 

OUTPUT 


CALL  GCOMP 


INPUT 

(T,  IENDF,  DTHETO,  DVA, 


DTHETZ) 

OUTPUT 


CALL  ALG 


INPUT 

(T,  IENDF,  DTHETZ,  DVA," 


DVN,  DCM 
OUTPUT 
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CALL  LLN 


INPUT^ 

(T,  IENDF , DVN,  ALTO,  DCM, 

NAVLAT,  NAVLON,  NAW,  NAVH,  NAVP , NAVR,  NAVHD) 


OUTPUT 


CALL  EVL 


INPUT.  (ALL)  ^ 

(T,  IENDF,  LAT,  LON,  ALT,  '/EL,  DVT,  PITCH, 

ROLL,  YAW,  WONDER,  NAVLAT,  NAVLON, 

NAW,  NAVH,  NAVP , NAVR,  NAVHD) 

After  each  of  the  CALL  statements,  a dummy  variable  IF1,  the 
switch  IENDF  returned  from  each  module,  is  tested.  If  IF1  = 1,  a 
flag,  SIMEND  = 1,  is  set,  otherwise  the,' next  CALL  statement  is  exe- 
cuted. (This  is  true  for  all  subroutines  except  the  trajectory  module.) 

The  four  modules,  TRAJ,  ENV,  GYRO,  ACCEL,  are  called  at  a fast 
cycle  — DT;  while  the  remaining  seven  modules  are  called  at  the  slow 
cycle  — DTSLOW. 

The  following  is  an  alphabetical  list  giving  definitions  to  the 
CALL  statement  variables. 


Variable 

Units 

Data  Type 

Description 

AB 

ft/s2 

REAL 

specific  force  vector  in  body  frame 
(from  TRJ  module) 

ABB 

ft/s2 

REAL 

specific  force  vector  in  body  frame 
(includes  vibrations  from  ENV  module) 

ALT 

ft 

REAL 

altitude  above  sea  level  (from  TRJ 
module) 

ALTO 

ft 

REAL 

indicated  altitude  above  sea  level 
from  the  altimeter 

DCM 

unity 

REAL 

direction  cosine  matrix 

DTHETA 

rad 

REAL 

vector  components  of  the  quantized 
integrated  change  in  the  X,  Y,  and 

Z gyro  input  angles  from  GYRO  module 
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/ariable 

Units 

Data  Type 

Description 

DTHETO 

rad 

REAL 

vector  components  of  the  quantized  inte- 
grated change  in  X,  Y,  Z gyro  input  angles 
in  body  frame  from  RDR  module 

DTHETZ 

rad 

REAL 

vector  components  of  the  compensated  quan- 
tized  integrated  change  in  X,  Y,  Z gyro  in- 
put angles  in  body  frame  from  GCOMP 

DV 

ft/s 

REAL 

vector  component  of  the  quantized 
integral  of  specific  force  in  the 
body  frame  (from  ACCEL  module) 

DVA 

ft/s 

REAL 

vector  components  of  the  compensated 
quantized  integral  of  specific  force 
in  the  body  frame  (from  ACOMP) 

DVN 

ft/s 

REAL 

vector  components  of  the  quantized 
integral  of  specific  force  in  the 
inertial  frame  (from  ALG  module) 

DVO 

ft/s 

REAL 

vector  components  of  the  quantized 
nominally  in  the  body  frame  (from 

RDR  module) 

DVT 

ft/s 

REAL 

vector  components  of  "true”  integral 
of  specific  force  in  ENU  frame  (from 

TRAJ  module) 

HDING 

rad 

REAL 

heading  (YAW  minus  WANDER  angle)  from  TRAJ 

IENDF 

logical 

INTEGER 

last-pass  indicator  all  modules 

LAT 

rad 

REAL 

geodetic  latitude  from  TRAJ 

LON 

rad 

REAL 

geodetic  longitude  from  TRAJ 

MODPDT 

s 

REAL 

module  print  interval  from  TRAJ 

NAVH 

ft 

REAL 

computed  navigational  altitude  from  LLN 

NAVHD 

deg 

REAL 

computed  navigational  heading  from  LLN 

NAVLAT 

deg 

REAL 

computed  navigational  latitude  from  LLN 

NAVLON 

deg 

REAL 

computed  navigational  longitude  from  LLN 

NAVP 

deg 

REAL 

computed  navigational  pitch  from  LLN 
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Variable 

Units 

Data  Type 

Description 

NAVR 

deg 

REAL 

computed  navigational  roll 

NAVV 

ft/s 

REAL 

computed  navigational  velocity  vector 
in  ENU  frame 

PITCH 

rad 

REAL 

pitch  angle  (second  rotation) 

PRNTSW 

logical 

REAL 

print  switch  (0  - no  printout, 
printout  otherwise) 

ROLL 

rad 

REAL 

roll  angle  (first  rotation) 

T 

s 

REAL 

current  simulation  time 

VEL 

ft/s 

REAL 

velocity  vector  in  body  frame 

WB 

rad/s 

REAL 

inertial-angular  rate  vector  in 
body  frame 

WBB 

rad/s 

REAL 

inertial-angular  rate  vector  in  body 
frame  (includes  vibrations  from  ENV 
module) 

WBBDOT 

2 

rad/s 

REAL 

inertial-angular  acceleration  vector 
in  body  frame  (includes  vibrations 
from  ENV  module) 

WANDER 

rad 

REAL 

wander  angle  (clockwise  from  north 
about  the  local  vertical  up  axis) 

YAW 

rad 

REAL 

yaw  angle  (third  rotation) 

The  following  logic  is  performed  at  the  end  of  each  sequencer 
operating  cycle. 

Check  for  simulation  end,  If  IENDF  = 1,  STOP 

If  T > TEND  or  If  SIMEND  > 0,  set  IENDF  = 1 and  make  one  last  pass 
through  each  of  the  modules. 

Increment  simulation  time,  and  correct  for  roundoff  error. 

T = T + DT 
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2.3.2  INTERIM  TRAJECTORY  MODULE  (TRAJ) 


(1)  General  Description 

This  module  is  a replacement  module  for  the  old  trajectory  module, 
the  latter  of  which  interfaces  the  AFAL  PROFGEN  (profile  generator)  pro- 
gram with  the  INS  Simulator.  It  is  used  to  generate  test  case  trajectory 
data  in  the  absence  of  the  PROFGEN  program.  Essentially,  the  function 
of  this  module  is  twofold: 

• The  generation  of  properly  coordinated  dynamic  data  to  drive 
the  simulated  gyro,  accelerometer,  and  altimeter  hardware 
modules. 

• The  provision  of  navigational  reference  information  (viz . , 
"true"  position  and  velocity  and  attitude)  to  evaluate  the 
performance  of  the  entire  system. 

(2)  Trajectory  Module  Flow  Diagram 

The  general  logic  flow  of  the  TRJ  module  is  shown  in  Figure  1. 

An  expanded  diagram  of  the  Print  Module  and  print  control  is  in- 
cluded in  Section  2,2  of  Volume  III* 

(3)  Input 

(a)  Module  Initialization  File  (IFILE) 

FORTRAN  unit  number:  20 


FORTRAN 

format:  15, 

F2  0.1,0 

Index 

Variable 

Default 

Value 

Units 

Description 

1 

DT 

0.01 

s 

module  operating  frequency 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print, 
otherwise  print) 

3 

OUTSW 

0.0 

logical 

not  used 

2-20 


Wi 


Figure  1.  Trajectory  module  (sheet  1 of  2) 


Figure  1.  Trajectory  module  (sheet  2 of  2). 
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Index 

Variable 

Default 

Value 

Units 

Description 

4 

XFILE 

6 

logical 

FORTRAN  unit  number  for 
printout 

5 

ILAT 

0.0 

deg 

initial  latitude 

6 

I LONG 

0.0 

deg 

initial  longitude 

7 

IALT 

0.0 

ft 

initial  altitude  above  sea 
level 

8 

IVEL(l) 

0.0 

ft/s 

initial  velocity  (VX)-EAST 

9 

I VEL ( 2 ) 

0.0 

ft/s 

initial  velocity  (VY) -NORTH 

10 

IVEL ( 3 ) 

0.0 

ft/s 

initial  velocity  (VZ)-UP 

11 

IPITCH 

0.0 

deg 

initial  pitch  angle 

12 

IYAW 

0.0 

deg 

initial  yaw  angle 

13 

IROLL 

0.0 

deg 

initial  roll  angle 

14 

MODPDT 

6.0 

s 

module  print  interval 

15 

IWANDR 

0.0 

deg 

initial  wander  angle 

(b) 

Common  Initialization  File  (PFILE) 

FORTRAN 

unit  number:  7 

FORTRAN 

format:  I5#  F20. 

10 

index 

Variable 

Default 

Value 

Units 

' Description 

1 

NE 

0. 72921 15147E-04 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTOT 

6.0 

s 

printing  frequency 

5 

LAT 

0.0 

deg 

initial  latitude 
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Index 

Variable 

Default 

Value 

Units 

Description 

6 

LON 

0.0 

deg 

initial  longitude 

7 

HANDER 

0.0 

deg 

initial  wander  angle 

8 

ALT 

0.0 

ft 

initial  altitude  above  sea 
level 

9 

ROLL 

0.0 

deg 

initial  roll  angle 

10 

PITCH 

0.0 

deg 

initial  pitch  angle 

11 

YAW 

0.0 

deg 

initial  yaw  angle 

12 

ROOT 

o.o 

rn  d/s 

first  time  derivative  of 
roll 

13 

PDOT 

0.0 

rad/s 

first  time  derivative  of 
pitch 

14 

YDOT 

0.0 

tad/s 

first  time  derivative  of 
yaw 

IS 

VEL(l) 

0.0 

ft/s 

initial  east  velocity 
wrt  the  earth  (VX) 

16 

VEL(2) 

0.0 

ft/s 

initial  north  velocity 
wrt  the  earth  (VY) 

17 

VEL(3) 

0.0 

ft/s 

initial  up  velocity  wrt 
the  earth  (VZ) 

18 

AB(1) 

0.0 

ft/i»“ 

longitudinal  specific 
force  in  body  frame  (AX) 

19 

AB(2) 

0.0 

ft/s2 

lateral  specific  force 
in  body  frame  (AY) 

20 

ABO) 

o.o 

ft/s2 

normal  specific  force 
in  body  frame  (A2) 
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2. J.3  ENVIRONMENT  MODULE  (ENV) 


(.1)  General  Description 

The  function  of  the  ENV  module  is  to  simulate  the  random  vehicle 
dynamics  (e.g.,  translational  acceleration  and  anqular  rates)  from 
sources  such  as  air  turbulence.  The  algorithm  used  in  this  module  takes 
a statistical  representation,  in  the  form  of  jiower-spectral  densities 
(PSDs) , of  linear  and  angular  displacement  vibrations,  then  computes  the 
appropriate  random  displacements  (linear  and  angular)  as  functions  of 
time.  The  algorithm's  numerical  differentiation  of  the  suitable  random 
displacement  function — twice  for  translational  acceleration,  once  for 
angular  rate — effectively  simulates  these  random  vibrations.  As  outputs 
of  the  ENV  module,  the  translational  and  angular  vibrations  are  subse- 
quently used,  in  combination  with  tin'  dynamics  transferred  by  the  tra- 
jectory module,  as  the  forcing  functions  of  the  INSS  system. 

Although  the  nominal  (default)  initialization  file  for  this  module 
uses  the  PSD  characteristics  of  a typical  low-altitude  B-l  (1-ft/s  gust 
standard  deviation)  mission  (sensed  in  the  forward  avionics  bay  randome) , 
the  user  may  establish  alternative  PSD  envelope  characteristics  in  this 
file.  An  additional  feature  of  this  module  is  a vibration  on/off  switch 
(VTBSW) , whose  value  is  specified  in  the  initialization  file  (defaulted 
to  the  "off"  position) . When  vibration  considerations  are  necessary, 
simulations  require  approximately  twice  as  much  computer  time  as  would 
be  the  case  if  the  ENV  module  were  bypassed. 

( 2 ) Environment  Module  Flow  Diagram 

The  general  logic  structure  of  the  ENV  modulo  is  displayed  in 
Figure  1 . Internal  chocks  indicating  cither  that  the  simulation  ter 
mination  time  has  been  reached  or  that  insufficient  time  has  elapsed 
for  the  next  iteration  cycle  will  force  an  immediate  return  to  the  main 
(sequencer)  program.  If  the  vibration  switch  is  set  to  the  off  posi- 
tion (VTHSW  - 0),  the  only  calculation  performed  before  returning  control 
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to  the  sequencer  is  the  determination  of  the  inertial  angular-acceleration 
components  in  the  body  frame.  The  first  initialization  pass  through  this 
module  (INITSW  = 0)  causes  the  initialization  .(IFILE)  and  physical  (PDATA) 
data  files  to  be  read,  coefficients  of  the  difference  equations  to  then 
be  calculated,  and  initialization  variables  to  be  written  before  finally 
returning  to  the  sequencer's  control.  The  calculations  invoked  by  sub- 
sequent passes  through  this  module  are  shown  in  the  accompanying  flow 
chart  (see  Figure  1) . 

(3)  Input 

(a)  Module  Initialization  File  (IFILE) 

FORTRAN  unit  number:  30 
FORTRAN  format:  15,  F20.10 


Index 

Variable 

Default  Value 

Units 

1 

DT 

0.01 

s 

2 

PRNTSW 

1.0 

logical 

3 

OUTSW 

0.0 

logical 

4 

XFILE 

6.0 

logical 

5 

S(l) 

4.0 

unity  1 

6 

S (2) 

3.0 

unity  1 

7 

S (3) 

0.0 

unity  1 

8 

S (4) 

3.0 

unity  / 

9 

S (5) 

4.0 

unity  1 

10 

S (6) 

1.0 

unity  1 

Description 

module  operating  frequency 

print  switch  (0  - no  print, 
otherwise  print) 

not  used 

FORTRAN  unit  number  for  printout 


number 
of  peaks] 
for  each ’ 
PSD  (5 
peaks 
per  PSD 
maximum 
allowed) 


normal 

lateral 


) 


linear 

Misplacement 


^vibration 
longitudinal 


pitch 

yaw 

roll 


/angular 

Misplacement 

Ivibration 
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Variables  11  - 40  are  peak  amplitudes  of  the  six  vibration  PSD  plots,  with 
up  to  five  peaks  per  plot  allowed  in  the  program*  These  amplitudes  are  for 
PSD  plots  normalized  to  unit  wind  gust  intensity. 


Index 

Variable 

Default  Value 

Units 

11 

PARMAP(l) 

136031. E-9 

ft2/Hz 

12 

PARMAP (2) 

15488. E-9 

ft2/Hz 

13 

PARMAP ( 3 ) 

1615. E-9 

ft2/Hz 

14 

PARMAP (4) 

17.5E-9 

ft2/Hz 

15 

PARMAP (5) 

0.0 

ft2/HZ 

16 

PARMAP (6) 

10304. E-9 

ft2/Hz 

17 

PARMAP (7) 

59. E-9 

ft2/HZ 

18 

PARMAP (8) 

3.08E-9 

ft2/HZ 

19 

PARMAP (9) 

0.0 

ft2/Hz 

20 

PARMAP (10) 

0.0 

ft2/Hz 

21 

PARMAP (11) 

0.0 

ft2/HZ 

22 

PARMAP (12) 

o 

• 

o 

ft2/HZ 

23 

PARMAP (13) 

0.0 

ft2/HZ 

24 

PARMAP (14) 

0.0 

ft2/HZ 

25 

PARMAP (15) 

0.0 

ft2/HZ 

26 

PARMAP (16) 

262000. E-12 

rad2/Hz 

27 

PARMAP (17) 

64088. 5E-12 

rad2/Hz 

28 

PARMAP (18) 

13226. E-12 

rad2/Hz 

29 

PARMAP (19) 

0.0 

rad2/Hz 

30 

PARMAP (20) 

0.0 

rad2/Hz 

Description 


I normal - 

\ linear 
I displacement 
1 vibration 


1 lateral- 
linear 

displacement 

vibration 


! longitudinal- 
linear 

displacement 

vibration 


! pitch- 
angular 
displacement 
vibration 
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Index 

Variable 

Default  Value 

Units 

Description 

31 

PARMAP (21) 

241902. 5E-12 

rad2/Hz 

N 

32 

PARMAP (22) 

24001. 5E-12 

rad2/Hz 

l yaw- 

33 

PARMAP (23) 

288 . 5E-12 

rad2/Hz 

. angular 

displacement 

34 

PARMAP (24) 

41.E-12 

2 

rad  /Hz 

| 

vibration 

35 

PARMAP (25) 

0.0 

2 

rad  /Hz 

j 

36 

PARMAP (26) 

72.E-6 

2 

rad  /Hz 

\ 

37 

PARMAP (27) 

0.0 

2 

rad  /Hz 

roll- 

38 

PARMAP (28) 

0.0 

rad  /Hz 

\ angular 

| 

displacement 

39 

PARMAP (29) 

0.0 

2 

rad  /Hz 

1 

l vibration 

40 

PARMAP (30) 

0.0 

2 

rad  /Hz 

) 

) 

Variables  41  - 70  are  half-bandwidths  of  PSD  peaks. 


41 

PARMWH (1) 

3.0 

rad/s  ^ 

42 

PARMWH (2) 

3.0 

rad/s 

normal- 

43 

PARMWH (3) 

6.0 

rad/s 

l linear 

displacement 

44 

PARMWH (4) 

4.0 

rad/s 

vibration 

45 

PARMWH (5) 

0.0 

rad/s  > 

46 

PARMWH (6) 

3.5 

rad/s 

| 

47 

PARMWH (7) 

8.0 

rad/s  | 

lateral- 

48 

PARMWH (8) 

6.0 

rad/s 

r linear 

displacement 

49 

PARMWH (9) 

0.0 

rad/s 

vibration 

50 

PARMWH (10) 

0.0 

rad/s  r 
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Index 

Variable 

Default  Value 

Units 

Description 

51 

PARMWH (11) 

0.0 

rad/s 

52 

PARMWH (12) 

0.0 

rad/s 

longitudinal- 

53 

PARMWH (13) 

0.0 

rad/s 

linear 

displacement 

54 

PARMWH (14) 

0.0 

rad/s 

vibration 

55 

PARMWH (15) 

0.0 

rad/s 

J 

56 

PARMWH (16) 

3.3 

rad/s 

57 

PARMWH (17) 

2.0 

rad/s 

pitch- 

58 

PARMWH (18) 

PARMWH (19) 

6.0 

rad/s 

rad/s 

l 

( 

angular 

^ displacement 
i vibration 

59 

• 0.0 

60 

PARMWH (20) 

0.0 

rad/s 

t 

61 

PARMWH (21) 

3.0 

rad/s 

) 

62 

PARMWH (22) 

5.0 

rad/s 

yaw- 

63 

PARMWH (23) 

9.0 

rad/s 

rad/s 

. angular 
( displacement 
l vibration 

64 

PARMWH (24) 

3.0 

65 

PARMWH (25) 

0.0 

rad/s 

J 

66 

PARMWH (26) 

2.0 

rad/s 

> 

\ 

67 

PARMWH (27) 

0.0 

rad/s 

1 roll- 

68 

PARMWH (28) 

0.0 

rad/s 

V angular 

/ displacement 

69 

PARMWH (29) 

0.0 

rad/s 

l vibration 

70 

PARMWH (30) 

0.0 

rad/s 

j 
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Variable  71  - 100  are  center  frequencies  of  PSD  peaks. 


Index 

Variable 

Default  Value 

Units 

Description 

71 

PARMO(l) 

13.5 

rad/s  \ 

72 

PARMO { 2 ) 

21.0 

rad/s  1 

normal- 

linear 

73 

PARMO ( 3 ) 

32.3 

rad/s  ) 

displacement 

vibration 

74 

PARMO (4) 

61 . 5 

rad/s  1 

75 

PARMO (5) 

0.0 

rad/s  / 

76 

PARMO (6) 

16.0 

rad/s  N 

77 

PARMO (7) 

34.0 

rad/s  | 

^ lateral- 

^ linear 

78 

PARMO (8) 

68.0 

rad/s 

displacement 

vibration 

79 

PARMO (9) 

0.0 

rad/s 

80 

PARMO (10) 

0.0 

rad/s  r 

81 

PARMO (11) 

0.0 

rad/s 

\ 

82 

PARMO (12) 

0.0 

rad/s 

1 

I longitudinal- 

83 

PARMO (13) 

0.0 

rad/s 

y linear 

displacement 

84 

PARMO (14) 

0.0 

rad/s 

vibration 

85 

PARMO (15) 

0.0 

rad/s  t 

86 

PARMO (16) 

13.5 

rad/s 

\ 

87 

PARMO (17) 

21.0 

rad/s 

1 pitch- 

! angular 

88 

PARMO (18) 

32.3 

rad/s 

/ displacement 

1 vibration 

89 

PARMO (19) 

0.0 

rad/s 

1 

90 

PARMO(20) 

0.0 

rad/s 

/ 
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Index 

Variable 

Default  Value 

Units 

91 

PARM0(21) 

3.0 

rad/s 

92 

PARMO{22) 

15.0 

rad/s 

93 

PARMO(23) 

33.0 

rad/s 

94 

PARM0(24) 

67.8 

rad/s 

95 

PARMO(25) 

0.0 

rad/s 

96 

PARMO(26) 

2.0 

rad/s 

97 

PARMO(27) 

0.0 

rad/s 

98 

PARMO{28) 

0.0 

rad/s 

99 

PARMO(29) 

0.0 

rad/s 

100 

PARMO(30) 

0.0 

rad/s 

101 

MODPDT 

6.0 

logical 

102 

VIBSW 

0.0 

logical 

103 

GUSTLA 

1.0 

-.2.  2 
ft  /s 

104 

GUSTLO 

c.o 

2 , 2 
ft  /s 

105 

GUSTNR 

1.0 

..2,  2 
ft  /s 

Description 


yaw- 

angular 

displacement 

vibration 


roll- 

angular 

displacement 

vibration 


module  print  interval 


vibration  switch  (0  - module 
bypassed) 


lateral 

longitudinal 

normal 


wind-gust 

intensity 

variable 


(b)  Common  initialization  File  (PFILE) 

FORTRAN  unit  number i 7 
FORTRAN  format:  15,  F20.10 


Index 

Variable 

Default  Value 

Units 

Description 

1 

WE 

0.7292115147E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 

PBUF{1) 

0.0 

not  used 
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M 


(4)  Call-Line  Data 

(?,— iendf^  AB,  WB," 


ABB,  WBB, 

WBBDOT 

(a) 

Call-Line  Input 

OUTPUT 

Variable 

Units 

Data  Type 

Description 

T 

s 

REAL 

current  simulation  time 

IENDF 

logical 

INTEGER 

last-pass  indicator 

AB 

ft/s2 

REAL 

specific- force  vector  in  body 
frame 

WB 

rad/s 

REAL 

angular  rate  vector  of  body  wrt 
inertial  space  in  body  frame 

(b) 

Call-Line  Output 

Variable 

Units 

Data  Type 

Description 

ABB 

ft/s2 

REAL 

specific- force  vector  in  body 
frame  with  vibration 

W3B 

rad/s 

REAL 

angular  rate  vector  of  body  wrt 
inertial  space  in  body  frame 
with  vibration  included 

WBBDOT 

rad/s2 

REAL 

angular  acceleration  vector  of 
body  wrt  inertial  space  in  body 
frame  with  vibration  included 

(5)  Formulation 

(a)  Initialization  Function 


The  switches  INITSW,  INI,  and  IN2  are  initialized  to  zero  in  DATA 
statements.  The  following  functions  are  performed  on  the  first  pass, 


if  INITSW  = 0 and  IENDF  = 0,  and  the  module  operating  cycle  time 
has  elapsed. 

• Read  and  print  initialization  file  (IFILE) . 

• Read  common  initialization  file  (PFILE) . 

• Set  OFILE  = XFILE. 

In  order  to  calculate  the  coefficients  of  the  difference  equation 
form  of  the  solution  of  the  normal  state  space  equation  (Vol.<II,  Sec.  4 
for  a theoretical  discussion) , the  following  (up  to  30)  terms  are  com- 
puted for  no  more  than  five  allowable  PDS  peaks  for  each  of  three  linear 
(normal,  lateral,  or  longitudinal)  and  three  angular  (pitch,  yaw,  or  roll) 
displacement  vibration  types. 

The  number  of  PSD  peaks  are  specified  by  S(J),  where  subscript  J 
identifies  the  displacement  vibration  type. 

The  first  term  in  each  of  two  uncorrelated,  zero  mean,  unity  vari- 
ance Gaussian  random  sequences,  ETA  and  ZETA,  are  computed  for  each  of 
the  possible  30  PSD  peaks. 

Then,  the  following  variables  for  each  of  the  K-PSD  peaks  (s  PSD's 
times  6 displacement  types)  are  defined.  First,  multiply  the  PSD  peak' 
amplitude  per  unit  wind-gust  intensity  by  gust  intensity  variance  product 
corrected  for  units. 

AP  «•  21T  * PARMAP(K)  * GUST(J) 

where 

J = 1 to  6 for  the  six  displacement  vibration  types 

# * 

with 


GUST(l) 

» GUSTNR 

GUST(2) 

« GUSTLA 

GUSTO) 

■ GUSTLO 

the  normal-linear  vibration, 
the  lateral-linear  vibration, 
the  longitudinal-linear  vibration. 
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GUST (4)  = GUSTNR,  the  effect  on  pitch-angular  vibration  cf 

the  normal-linear  vibration. 

GUST (5)  = GUSTLA,  the  effect  on  yaw-angular  vibration  of  the 

lateral-linear  vibration. 

GUST (6)  = GUSTLA,  the  effect  on  roll-angular  vibration  of  the 

lateral-linear  vibration. 


Then,  define  the  half- power  bandwidth  to  central  frequency  ratio 
of  each  PSD  peak. 

A = WH/WO 

where  ' 

WH  » PARMWH (K) 

WO  = PARMWO(K) 

Now,  the  undamped  natural  frequency  is 

WN  « WO*  (2  -(1  - a2/2)2)1//4 

The  damping  ratio  is 

E = ((1  - (WO/WN)2/2)1/2 

and  the  filter  gain  is 

P » (l  -U  - 2eV)*AP 

Initialize  the  displacements  of  previous  pass  for  the  K-PSD  peaks  as 

XIPRV(K)  » 0.0 
X2PRV (K)  o 0.0 
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Calculate  the  resonant  frequency 


WR 


WN* (1  - E2)1/2 


For  convenience  and  economy  of  notation,  the  following  variables 
are  defined 

Z = E*WN 

El  = EX?(-2*Z*DT) 

C = AS  IN  (E) 

/ 

U - (1  - E1)/(4*Z*WR2) 

+ (El* (Z*CQS (2*WR*DT)-  WR*SIN (2*WR*DT) ) )/ (4*WN2*WR2) 

2 

- E/(4*WN*WR  ) 

R = (1  - E1)/((4*E*WR2)/WN) 

- (El*  C2  *C0S(2*WR*DT  + 2*0  - WR*SIN  (2*WR*DT  + 2*C) ) ) ,/4*  WR2) 
+ (Z*C0S(2*C)-  WR*SIN(2*C))/(4*WR2) 

V =>  (FI.  - 1)  *STN(C)/(4*E*WR2) 

2 

- (El* (Z*SIN (2*WR*DT  + C)  + WR*COS (2*WR*DT  + C)))/4*WN*WR  ) 

+ (WR*COS(C)+  Z*SIN(C))/(4*WN*WR2) 


Finally,  the  required  coefficients  of  the  difference  equation  form 
of  the  solution  of  the  normal  state  space  equation  are  computed.  For 
each  PSD  peak  the  set  of  coefficients  computed  is 


B » (|  0 - (V2/R)|)1/2*F1/2*WN2 

H a (V/R1/2)*F1/2*WN2 

p ti  r1/2*f1/2*wn2 

P12  W (SIN(WR*DT)*EXP(-Z*DT>)A»R 

P22  » COS  (WR*DT)  - (Z/WR)  *SIN(WR*DT)  *EXP(-Z*DT) 

Pll  » 2*Z*P12  + P22 

P21  » -WN2*P12 
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where  B,  H,  P are  the  coefficients  of  the  noise  terms  ETA  and  SETA 
,and  Pll,  P12,  P21,  and  P22  are  elements  of  the  transition  matrix. 

The  K'th  set  of  coefficients  is  stored  as  the  K'th  elements  in  a 
set.  of  coefficient  array  variables. 

C0EF1 (K)  = B 

C0EF2 (K)  = H 

C0EF3(K)  = P 

C0EF4 (K)  = P12 

G0EF5 (K)  « P22 

C0EF6 (K)  = Pll 

C0EF7 (K)  » P21 

Now  set  the  specific  force,  the  body  inertial-angular  rate,  and 
the  body  inertial-angular  acceleration  in  the  body  frame  as 

WBB  « m 
ABB  e AB 

wbbSot  = <0.0,  0.0,  0,0) 

PRINT  out  the  initialisation  data. 

Set  INITSW  « 1 and  increment  the  simulation  time 

TEMV  = T + DT 

and  return  to  the  main  program. 

(b)  General  Function 
If  the  vibration  switch 

V2BSW  < 0 


then  the  vibration  computation  is  bypassed  and  the  inertial  angular 
rate  of  the  previous  pass  is  defined  as 


WBBOLD  = WBB 

and  then 

WBB  = WB 
ABB  = AB 

WBB DOT  = (WBB  - WBBOLD) /DT 

The  results  are  printed#  the  simulation  time  is  incremented 

TENV  + DT 

And  there  is  a return  to  the  main  program - 
If  the  vibration  switch 

VIBSW  > 1 

The  module  generates  sequences  of  random  numbers  ETA  and  ZETA,  to 
force  the  linear  difference  equations,  and  Y is  initialized  to  zero 
for  each  of  the  PSDs. 

The  random  processes  XI (I)  and  X2(I)  are  generated  for  each  of 
the  I spectral  density  peaks  for  each  single  displacement  vibration  type 

XI  (I)  * PlFXlPRV(I)  + P12*X2PRV(I)  + H*ETA  + B*ZETA 
X2  (I)  - P2PX1PRV(I)  + P22*X2PRV (I)  + P*ETA 
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and  solved  for  the  random  process,  XI (I).  On  the  first  pass  after 
initialization,  when  XN2  » 0,  each  value  of  XI (I)  is  divided  by  the 
module  operating  frequency  to  obtain  a velocity,  and  that  velocity 
is  summed  over  the  power  spectral  density  peaks  in  a summation  on  the 
index  variable  I 


Y =»  Y + X1(I)/DT 

On  succeeding  passes,  the  above  summation  is  replaced  with 

Y = Y + (XI (I)  - X1PRV (*) ) /DT 

The  Y variable  is  controlled  in  a summation  with  index  J which  identifies 
the  J'th  value  of  Y as  the  J'th  vibration  type.  The  J'th  value  of  Y is 
inserted  as  the  J'th  element  of  the  array  variable  RAND  via 

RAND (J)  » Y 

for  each  of  the  six  displacement  vibration  types. 

The  values  XI (1)  and  X2(I)  are  stored  in  array  variables  repre- 
senting the  previous  values  for  the  next  solution  loop 

XIPRV(K)  = XI (I) 

X2PRV (K)  - X2(I) 

and  IN2  is  set  to  1.  This  completes  the  vibration  generation. 

Now  the  environment  output,  angular  rates,  and  specific  forces, 
which  include  vibration- induced  motions,  are  calculated.  First,  set 
the  body  angular  rate  with  vibrations  from  previous  pass  to 

WBBOLD  - WBB 
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Then,  the  three  linear-displacement  vibrations 


DELVB  = first  three  components  of  RAND 

Now,  compute  the  change  in  specific  force  due  the  vibration 

DELAB  = (DELVB  - DELVB P) /DT 

where  DELVBP  is  the  DELVB  of  the  previous  pass. 

Now  redefine 


DELVBP  = DELVB 

the  DELVBP  as  the  BELVB  of  the  previous  pass  for  the  next  pass. 

The  three  angular-displacement  vibrations  are  defined  as 

DELWB  «*  last  three  components  of  RAND 

i.e.,  the  angular-displacement  vibrations  of  pitch,  yaw,  and  roll, 
respectively. 

The  components  of  the  angular-displacement  (pitch,  yaw,  and  roll) 
vibrations  are  added  to  the  angular  rates  in  the  body  frame 

WBB(l)  *=  WB  (1)  + DELWB  (3) 

WBB (2)  = WB (2)  + DELWB ( 1 ) 

WBB (3)  = WB ( 3)  + DELWB ( 2 ) 
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The  transformation  matrix  that  establishes  the  turbulence- induced 
change  in  body  attitude  is  computed  from 


QBBCHG 


■“  1 

a 

— 

» 

1 

1 

(DELWB  (2)  , 

(DELWB (1) 

1.0  • 

I 

1 

1 

+ DELWBP  (2) ) /2DT, 

1 

+ DELWBP (1) )/2DT 

I 

- (DELWB (2)  » 

1 

i 

1 

1.0  1 

(DELWB (3) 

+ DEWBP(2) ) /2DT  » 

1 

i 

1 

1 

+ DELWBP (3) ) /2DT 

i 

(DELWB (1) 

i 

-(DELWB (3)  • 

1 

1.0 

+ DELWBP  (1) ) /2DT  * 

+ DELWBP (3) ) /2DT1 

1 

where  the  terms  of  the  matrix  are  the  vibration-induced  rate,  and  its 
previous  pass  value  averaged  and  used  over  the  time  increment. 

Then,  the  total  change  in  body  attitude  is  given  by 


* * * 

QBB  = QBBCHG  * QBBPRV 


* * 

where  QBBPRV  «=  QBB  of  the  previous  pass.  On  the  first  pass  after 
initialization 


QBB*PRV  ■ I 

from  a computation  time  initialization. 

The  terms  of  the  vibration-induced  angular  rate,  DELWB,  from  the 
current  pass  are  stored  as  DELWBP 

t 

DELWBP  - DELWB 
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Now,  the  attitude  matrix,  QBB,  is  orthonormalized.  First,  compute 
. , * 
the  cross-product  matrix,  QTQ, 

* 3 * * T * 

QTQ  = ( I - OBB  * 0BB)/2 

He 

where  I is  the  identify  matrix. 

* 

Orthonormalization  of  QBB  is  complete  with  the  computation 

* ★ * 

QBB  = QBB  * QTQ 

Next,  the  rotated  specific  force  is  computed 

ABB  = QBB  * AB 

and  the  nominal  gravity  times  the  linear-displacement  vibrations  for 

longitudinal,  lateral,  and  normal  displacements  are  added  -to 

the  body  frame  components  of  the  specific  force  without  vibration 

ABB ( 1)  = ABB (1)  + DELAB (3) *G 

ABB (2)  = ABB (2)  + DELAB (2) *G 

ABB (3)  = ABB (3)  + DELAB (1) *G 

* * 

The  attitude  matrix,  QBB,  of  the  current  pass  is  stored  as  QBBPRV 

for  use  in  the  next  pass. 

* * 

OBBPRV  = QBB 

Finally,  the  angular  accelerations,  WBBDOT,  are  calculated 
WBBDOT  - (WBB  - WBBOLD) /T 

Output  is  printed,  and  the  simulation  time  is  incremented 

TENV  = T + DT 

and  there  is  a return  to  the  main  program. 
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(6)  Output 


(a)  Print 


FORTRAN  unit  number:  OFILE  = 6 

On  the  initialization  pass,  the  title,  "ENVIRONMENT  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  >_  1.  See  Section  2.2  for  print  control  logic.  The  printed 
output  is,  as  follows: 


Variables 

Units 

Description 

AB 

2 

ft/s 

specific- force  vector  in  body 
frame  without  vibrations 

WB 

rad/s 

angular-rate  vector  in  body 
frame  without  vibrations 

ABB 

rad/s 

specific- force  vector  in  body 
frame  with  vibration  included 

wIb 

rad/s 

angular-rate  vector  in  body 
frame  with  vibration  included 

WBBDOT 

rad/s2 

angular-acceleration  vector  in 
body  frame  with  vibration 
included 

(7)  Subroutines  Called  (see  Section  2.3.15) 

MXV  = matrix  by  vector  product 

MXM  = matrix  multiplication 

MTXM  = matrix  transpose  by  matrix  product 

GAUSS ( AM, SD)  = Gaussian  random  number  generator 
function  with  mean,  AM,  and 
standard  deviation,  SD. 
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2.3.4  GYRO  MODULE  (GYROS) 

(1)  General  Description 

The  GYRO  module  simulates  the  performance  of  three  conventional 
single-degree-of-freedom  (SDF)  rate  integrating  gyros.  Instrument 
kinematics  are  modeled  on  the  basis  of  one  of  three  available  (user- 
specified)  differential  equations:  a "performance"  model  (without 
gyro  inertia  or  damping  terms) ; a first-order  differential  equation 
(with  only  gyro  damping) ; and  a second-order  differential  equation 
(with  both  gyro  damping  and  inertia) . Also  simulated  in  this  module 
are  the  following  gyro  sensitivities: 

• Acceleration  sensitivity  (both  g and  g-squared  terms) . 

• Instrument  misalignments. 

• Anisoinertia. 

• Output-axis  coupling. 

• Quantization  levels . 

• Scale-factor  error:,  (both  positive  and  negative)  . 

• Scale-factor-error  rate  sensitivity  (both  positive  and 
negative) . 

• Bias. 

• Bias  transient  at  turn-on. 

• Random  bias  (exponentially  correlated) . 

Although  the  user  may  select  any  alternate  gyro  parameters  by  modifying 
the  attendant  initialization  data  file,  the  nominal  (default)  parameters 
used  by  this  module  correspond  to  the  CSDL  18  IRIG  Mod  B gyro  in  an 
analog  rebalance  loop. 

(2)  Gyro  Module  Flow  Diagram 

The  general  flow  logic  of  the  gyro  module  is  illustrated  in 
Figure  1. 
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Figure  1.  Gyro  Module  (sheet  1 of  2) . 


Vol  III  Sec  2.3.4 
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Figure  1.  Gyro  Module  (sheot  2 of  2) . 
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(3)  Input 


(a)  Module  : 

Initialization 

File  (IFILE) 

Index 

FORTRAN  unit  number: 

FORTRAN  format:  15, 

Variable  Default  Value 

40 

F20.10 

Units 

Description 

1 

DT 

0.01 

s 

module  operating  timestep 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print. 

3 

OUTSW 

0.0 

otherwise  print) 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for  printout 

5 

SPAREl 

0.0 

not  used 

6 

SPARE2 

0.0 

not  used 

7 

CO 

7000000.0 

dyn-cm/ 

gyro  deunping  coefficient 

8-12 

rad/s 

not  used 

13 

QGBX(l) 

1.0 

unity  \ 

14 

QGBX ( 2 ) 

0.0 

unity 

15 

QGBX ( 3 ) 

0.0 

unity 

16 

QGBX (4) 

0.0 

unity  I 

X-gyro  transformation  matrix 

17 

QGBX ( 5 ) 

0.0 

unity  y 

, from  body  coordinates  to  gyro 

* (input,  output,  spin)  coordinates 

18 

QGBX (6) 

1.0 

unity  l 

19 

QGBX(7) 

0.0 

unity 

20 

QGBX(8) 

-1.0 

unity 

21 

QGBX (9) 

0.0 

unity  ^ 
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Index 

Variable 

Default  Value 

Units 

Description 

22 

QGBY(l) 

0.0 

1 

unity 

23 

QGBY (2) 

1.0 

unity 

24 

QGBY(3) 

0.0 

unity 

1 

25 

QGBY (4) 

1.0 

unity  f 

Y-gyro  transformation  matrix 

26 

QGBY (5) 

0.0 

unity  / 

> from  body  coordinates  to  gyro 
(input,  output,  spin)  coordinates 

27 

QGBY (6) 

0.0 

unity  1 

28 

QGBY (7) 

0.0 

unity  ' 

29 

QGBY (8) 

0.0 

unity 

30 

QGBY (9) 

-1.0 

unity  j 

31 

QGBZ(l) 

0.0 

unity  \ 

32 

QGBZ (2) 

0.0 

unity 

33 

QGBZ (3) 

1.0 

unity  | 

34 

QGBZ (4) 

1.0 

unity 

f 

Z-gyro  transformation  matrix 

35 

QGBZ(5) 

0.0 

unity 

> from  body  coordinates  to  gyro 

(input,  output,  spin)  coordinates 

36 

QGBZ (6) 

0.0 

unity 

i 

37 

QGBZ (7) 

0.0 

unity 

1 

38 

QGBZ (8) 

1.0 

unity 

39 

QGBZ (9) 

0.0 

unity 

t 

/ 

40 

H 

151000.0 

2 

gm-cm  /s  angular  momentum  of  gyro  wheels 

41 

— 

not  used 

42 

QUANT 

0.00007 

arcsoc 

quantization  level  of  digitized 
change  in  angle 
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Index  Variable  Default  Value 


Units 


Description 


43 

BIAS (1) 

-0.31 

deg/h 

44 

BIAS (2) 

-0.31 

deg/h 

45 

BIAS (3) 

-0.31 

deg/h 

46 

K 

3100000000.0 

2. 

gm-cm  /s 

47 

I 

226.0 

2 

gm-cm 

48 

DELI 

14.0 

2 

gm-cm 

49 

— 

50 

KI(1) 

1.12 

deg/h/g 

51 

KI  (2) 

1.12 

degA/g 

52 

KI  (3) 

1.12 

deg/h/g 

53 

KO  (1) 

-0.032 

degA/g 

54 

KO  (2) 

-0.032 

deg/h/g 

55 

K0(3) 

-0.032 

degA/g 

56 

KS(1) 

-0.62 

deg/h/g 

57 

KS  (2) 

-0.62 

deg/h/g 

58 

KS  (3) 

-0.62 

deg/h/g 

59 

KII(l) 

0.0 

deg/h/g2 

60 

KII (2) 

0.0 

degA/g2 

61 

KII (3) 

0„0 

deg/h/g2 

62 

KSS  ( 1.) 

-0.030 

deg/h/g2 

63 

KSS (2) 

-0.030 

deg/h/g2 

64 

KSS (3) 

-0.030 

deg/h/g^ 

X-gyro  \ 

Y-gyro  > bias  levels 
Z-gyro  / 

rebalance  loop  elastic  restraint 

gyro  output  axis  moment  of 
inertia 

gyro  spin  axis  minus  input  axis 
moment  of  inertia 


not  used 


input  axis 

acceleration-sensitive 
coefficients  for 


output  axis 

acceleration-sensitive 

coefficients  for 


spin  axis 

acceleration-sensitive 
coefficients  for 


input  axis  x input  axis 
acceleration-squared 
sensitive  coefficients 
for 


spin  axis  x spin  axis 
acceleration-squared 
sensitive  coefficients 
for 


X-gyro 

Y-gyro 

■ 

, Z-gyro 

r X-gyro 

Y-gyro 

{ Z-gyro 

[ X-gyro 

Y-gyro 

l Z-gyro 
r 

X-gyro 

i 

Y-gyro 

Z-gyro 

f Y-gyro 

1 

1 Y-gyro 
I Z-gyro 
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Index 

Variable 

Default  Value 

Units 

05 

K10(1) 

0.0 

deg/h/g2 

66 

KI0 (2) 

0.0 

deg/h/g2 

67 

KI0 (3) 

0.0 

deg/h/g2 

08 

KIS (1) 

0.21 

deg/h/g2 

69 

KIS (2) 

0.21 

deg/h/g2 

70 

KIS (3) 

0.21 

deg/h/g2 

71 

K0S(1) 

-0.0075 

deg/h/g2 

72 

KOS ( 2 ) 

-0.C075 

deg/h/g2 

73 

KOS ( 3 ) 

-0.0075 

deg/h/g2 

74 

BIASV(l) 

0.0 

(deg/h) 2 

75 

BIASV (2) 

0.0 

(deg/h) 4 

76 

BIASV (3) 

0.0 

(deg/h) 4 

77 

SFPO(l) 

-20.0 

ppm 

78 

SFPO ( 2 ) 

-20.0 

ppm 

79 

SFPO (3) 

-20.0 

ppm 

80 

SFMO(l) 

-54.0 

ppm 

81 

SFMO ( 2 ) 

-54.0 

ppm 

82 

SFMO (3) 

-54.0 

ppm 

Description 


input  axis  x output  axis 
acceleration-squared 
sensitive  coefficient 
for 


X-gyro 

Y-gyro 

Z-gyro 


input  axis  x spin  axis 
acceleration-squared 
sensitive  coefficient 
for 


1 X-gyro 
Y-gyro 
Z-gyro 


output  axis  x spin  axis 
acceleration-squared 
sensitive  coefficient 
for 


variance  of  random  bias 
(exponentially  corre- 
lated) for 


positive  scale-factor 
errors  for 


X-gyro 

Y-gyro 


negative  scale-factor 
errors  for 


^Z-gyro 

( X-gyro 
Y-gyro 
Z-gyro 
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Index 

Variable 

Default  Value 

83 

MODPDT 

6.0 

84 

ORDER 

0.0 

85 

TRANSl (1) 

0.0 

86 

TRAN SI (2) 

0.0 

87 

TRANSl (3) 

0.0 

88 

TRANTC ( 1 ) 

200.0 

89 

TRANTC ( 2 ) 

200.0 

90 

TRANTC (3) 

200.0 

91 

SFPI (1) 

0.0 

92 

SFPI(2) 

0.0 

93 

SFPI (3) 

0.0 

94 

SFMI(l) 

0.0 

95 

SFMI (2) 

0.0 

96 

SFMI (3) 

0.0 

97 

BIASTC 

40.0 

Units  Description 


s 


module  print  interval 


logical  order  of  differential  equation 
model  (0  = performance  model, 

1 ~ first-order  differential 
equation,  2 = second-order 
differential  equation) 


dcg/h 

deg/h 

deg/h 


ppm/rad/ s 
ppm/rad/s 
ppm/rad/s 
ppm/rad/s  I 
ppm/rad/ s > 
ppm/rad/s 


) 


exponential  decay 
turn-on  bias 
initial  values  for 


exponents  decay 
turn-on  bias 
time  constant  for 


positive 

scale-factor  errors 
varying  with  rate  for 


negative 

scale- factor  errors 
varying  with  rate  for 


X-gyro 
Y-gyro 
2- gyro 
X-gyro 
Y-gyro 
z- gyro 
X-gyro 
Y-gyro 
Z-gyro 
X-gyro 
Y-gyro 
Z-gyro 


s exponentially  correlated  random 

bias  time  constant  (must  be 
nonzero) 


(b)  Cotrenon  Initialization  File  (PFILE) 


FORTRAN  unit  number:  7 
FORTRAN  format:  15,  F20.10 
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Index 

Variable 

Default  Value 

Units 

Description 

1 

WE 

0 . 7292115147E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 

PBUF(l) 

0.0 

not  used 

(4)  Call-Line  Data 


^INPUT^ 

(T,  IENDF,  WBB , WDOT,  ABB', 


DTHETA  ) 
OUTPUT 


(a)  Call-Line  Input 


Variable 


Units  Data  Type 


Description 


T 


S 


REAL 


current,  simulation  time 


IENDF  logical  INTEGER  last- pass  indicator 

WBB  rad/s  REAL  angular  rate  vector  of 

body  wrt  inertial  space 
in  body  frame  with 
vibration  included 

— 2 

wdot  rad/s  REAL  angular  acceleration 

vector  of  body  wrt 
inertial  space  in  body 
frame  with  vibration 
included  (WBBDOT) 


ABB 


REAL 


specific  force  in 
body  frame  with 
vibration  included 
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(b)  Call-Lino  Output 


Variable  Units  Data  Typo  Description 

DTHETA  rad  REAL  quantized  integral  of  angular 

rate  about  X,  Y,  Z gyro  input 
axes  over  the  computation 
cycle,  plus  errors 


(5)  Formulation 


(a)  Initialization  Function 

The  switch  INITSW  is  initialized  to  zero  in  DATA  statement.  The 
following  functions  are  performed  on  the  first  pass,  if  INITSW  = 0 and 
IENDF  = 0 and  the  module  operating  cycle  time  has  elapsed. 

• Read  and  print  initialization  file  (I FILE) . 

• Read  common  initialization  file  (FFILE) . 

• Set  OF  ILF.  = XFILE 

and  NORDER  = ORDER. 

• Convert  initialization  data  to  computational  units 


sro 

= 

SFTO* l . E-G 

SPl 

= 

SFP1  *1  . E-G 

SMI 

= 

sfmi*i.e-:> 

SMO 

- 

5FM0* 1 . E-o 

AQUANT 

QUANT *n/G . 4RE5*II/DT/K 

EX 

- 

EXF (-DT/BIASTC) 

CO 

CO*2 . 37F.-6 

DELI 

DEI. 1*2 . 37E-G 

K 

- 

K*2 . 37E-G 

1 

E3 

1*2 . 37F.-G 

DTI 

Km 

DT/I 

H 

tar 

11*2 . 37K-6 

TRANS 1 

« 

TRANS 1 * 4 . 86E- 6 

HI  AS  A 

C2 

(nTASV*(l  - EX2))1/2*4.B5E-G 

HI  AS 

IS 

HTAS*4.m[»E-G 

2-1 J 


In  addition  to  converting  to  computational  units,  the  following 
variables  are  tested  and  the  switches  Kll  and  K12  (which  were  initialized 
to  zero  in  the  DATA  atatement)  are  reset. 


If 

BIASV 

¥ 0,  set  Kll  = 1 

If 

kT 

¥ 

0,  set  K12  = 1 

If 

KO 

¥ 

0,  set  K12  = 1 

If 

KS 

¥ 

0,  set  K12  - 1 

If 

m 

¥ 

0,  set  K12  = 1 

If 

KSS 

¥ 

0,  set  Kl2  = 1 

IF 

kTo 

¥ 

0,  set  K12  = 1 

KI 

= 

KTM.85E-6/G  = 1 

KO 

= 

KOM85E-6/G  = 1 

KS 

= 

KS*4. 35E-6/G  = 1 

kTi  = 

KIIM.85E-6/G2  = 1 

KSS  = 

2 

KSS*485E~6/G  = 1 

k7o  = 

2 

KIOM.85E-6/G  = 1 

Initialization  is  now  complete,  INITSW  is  set  to  1,  and  the  simula- 
tion time  is  incremented 


TGYR  = T + DT 

Now,  the  routine  returns  to  the  main  program. 

(b)  General  Functions 

The  following  functions  are  performed  every  module  operating  cycle. 
The  quantized  integrated  change  in  the  input  angle  of  the  X,  Y, 
Z-gyros,  THETA,  is  computed  in  turn. 

First  for  the  X-gyro,  when  II  * 1,  the  coordinates  of  specific 
force,  angular  rate,  and  angular  acceleration  are  transformed  from 
(XYZ)  body  coordinates  to  (IDS)  gyro  coordinates 


★ 

__ 

ABG 

55 

OGRX 

* ABB 

WBG 

s= 

* 

OGBX 

* WBB 



* 

— 

WBDOT 

OGBX 

* WDOT 

the  Y-gyro,  when 

H 

H 

2, 

* 

ABG 

ss 

OGBY 

* ABB 



★ 

WBG 

= 

QGBY 

* WBB 

— 

it 

WBDOT 

= 

OGBZ 

* VTK0T 

when  11  = 3 

★ 

ABG 

s= 

OGBZ 

* ABB 

"''V  — 

* 



X WBG 

= 

OGBZ 

* WBB 

S_ 

* 

WBDOT 

ss 

OGBZ 

* WDOT 

Now,  for  tyros  X,  Y,  and  Z,  when  II  = 1,  2,  and  3 respectively, 
the  following  terms  are  calculated. 

If  on  the  first  pass  after  initialization  (KlO  = 0) , set 


WBDOT ( 2 ) = 0.0 


and  the  output  axis  value 


THETA (II)  = WBG(l)  * H/K 

where  WBG(l)  is  the  angular  rate  along  the  input  axis. 

The  bias  levels  are  now  calculated.  Fisrt,  if  the  bias  variance 
is  zero,  BIASV(Il)  = 0.0  (Kll  =0).  Then,  the  random  number  generation 
is  bypassed,  and  the  random  bias  is  set  to  the  bias  level 

BIA  = BIAS ( I 1 ) 
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Otherwise,  a Gaussian  random  number  with  zero  mean  and  a standard 
deviation  equal  to  BI  = BIAS(ll)  is  generated  and  the  exponential 
correlated  random  bias,  BIA , is  calculated.  The  value  of  GBIAS 
for  the  particular  gyro  from  the  previous  pass  (on  the  first  pass 
after  the  initialization,  GBIAS  = (0.0,  0.0,  0.0)  is  multiplied 
by  the  exponential  function,  EX,  and  added  to  the  random  number 

GBIAS  (ID  = GBIAS  (I'D  *EX  + GAUSS  (AM, BI) 

where 

EX  = EXP (-DT/BIASTC) 

Finally,  the  exponentially  correlated  random  bias  is 

BIA  « BIAS  (ID  + GBIAS  (II) 


The  single— degree-of- freedom  gyro  module  dynamic  equations  are 
solved  for  the  float  offset  angles,  THETA,  for  any  one  of  three  approxi- 
mate forms: 

• The  performance  model  when  NORDER  = 0. 

• The  first-order  model  when  NORDER  =1. 

• The  second-order  model  when  NORDER  = 2. 

See  Vol  II,  Sec  5.1.1  for  complete  description.. 

A quantity  DEN  is  defined 

DEN  = K + DELI*  (V7BG(3)2  - WBG(l)2)  + H*WBG(3) 

where  the  subscripts  3 and  1 denote  the  spin  and  input  axes,  DEN  con- 
sists of  three  torque  terms.  These  terms  are  the  rebalance  loop 
elastic  restraint  torque,  the  anisoinertia  coupling,  and  the  cross- 
coupling torque  per  unit  float  offset. 
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Further  we  define  Ml  consisting  of  the  torque  terms:  input, 
random  bias,  exponentially  decaying  turn-on  bias,  torque  due  to 
Coriolis  effects,  and  output  axis  angular-acceleration  torque 

Ml  = H* (WBG (1)  - BIA  + TRANS1 ( II) *EXP9-T/TRANTC (11) ) ) 

+ DELI*WBG(3) *WBG(1)  - I*WBD0T(2) 

Additional  torque  terms  in  the  numerator,  denoted  by  M2  are 
computed  for  acceleration  and  acceleration-squared  sensitive  effects, 
if  any  of  the  coefficients  are  nonzero  or  when  K12  = 1 

M2  = -H*(KI (II) *ABG(1)  + KS (11) *ABG(3)  + KO (11) *ABG (2) 

+ KSS (II) *ABG(3) 2 + KII (II) *ABG(1) 2 
+ KIS (11) *ABG(1) *ABG(3)  + KIO(Il) *ABG(1) *ABG(2) 

+ KOS (II) *ABG(2) *ABG(3) ) 

Otherwise,  M2  * 0.0. 

If  NORDER  = 0,  the  performance  model  equation  is  solved  for  the 
float  offset  angle,  THN 


THN  = (Ml  + M2) /DEN 

If  NORDER  = 1,  from  the  first-order  model  equation,  THN  is 

THN  = THETA (11)  + DT* (- DEN* THETA (11)  + Ml  + M2) /CO 

If  NORDER  = 2,  from  the  second-order  model  equation,  THN  is 

THN  = THETA (II)  + DT*THD0T(I1) 

where  THDOT(Il)  is  the  previous  pass  value  which  is  initially  zero. 
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The  term  THDOT  is  calculated  for  use  in  the  next  pass  from  the  expres- 
sion 


THDOT(Il)  = THDOT (II)  + (Ml  4-  M2  - C0*THD0T(I1) 

- DEN*THETA ( II ) ) *DTT 

Finally,  the  quantized  integral  of  angular  rate  about  the  gyro 
input  axes,  DTHETA,  are  calculated  for  the  X,  Y,  or  Z gyro  depending 
upon  the  value  of  the  index,  II. 

First,  the  float  offset  angle  is  added  to  the  residual  output 
angle  from  previous  quantization 

TTHET(Il)  = THN  + TTHET(Il) 

Scale  factor  errors  are  accounted  for  in  the  case  of  positive 
offset  angle  by  defining 

T1  = 1 + SPO(Il)  + SPl (II) *THN*K/H 

or  in  the  case  of  negative  offset  angle  by  defining 

Tl  = 1 + SMO (II)  + SMI (11) *THN*K/H 

Using  this  definition  to  account  for  the  scale-factor  error,  the  integer 
variable, 

ITHET  = TTHET(Il) /(AOUANT*Tl) 

is  computed.  It  expresses  how  many  quantum  steps  of  angle,  AQUANT, 

are  contained  ITHET  and  represents  the  increment  in  pulse  count  from 
a digital  rebalance  gyro. 
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When  forming  the  integer  pulse  count,  ITHET,  any  fractional  part 
of  a quantum  step  is  dropped.  This  fractional  part  represents  a residual 
float  offset  which  should  be  retained  for  the  next  computation  cycle. 

The  residual  offset  angle  is  retrieved  as 


TTHET(Il)  = TTHET(Il)  - TT*Tl*AQUANT 


where 

TT  = FLOAT (ITHET)  (Keeps  the  FORTRAN  arithmetic  in  real  variables). 


The  offset  angle  THN  is  renamed 


THETA  (ID  = THN 


for  use  in  the  next  THN  computation. 

Now,  the  desired  output  from  the  gyro  module,  DTHETA,  is  obtained 
for  the  X,  Y,  and  Z gyros. 

DTHETA  (ID  = DTHETA  (ID  + TT*AQUANT*DT*K/H 

The  OTHETA  is  printed  and  the  THETA  angle  initialization  switch 
is  set  to  1,  KIO  = 1.  The  simulation  time  is  incremented 

TGYR  = T + DT 

and  the  subroutine  returns  to  the  main  program. 

(6)  Output 

(a)  Print 

FORTRAN  unit  number:  OFILE  “ 6 

On  the  initialization  pass  the  title  "GYROSCOPE  INITIALIZATION" 
and  the  initialization  data  are  printed. 
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Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  1.  See  Section  2.2  for  print  control  logic.  The  printed 
output  is,  as  follows 


Variables  Units  Description 

DTHETA,  rad  quantized  integral  of  angular  rate 

about  the  X,  Y,  Z gyro  input  axes 
over  the  computation  cycle,  plus 
errors 


(7)  Subroutines  Called  (See  Section  2.3.15) 

MXV  = matrix  by  vector  product. 

GAUSS (AM, BI)  = Gaussian  random  number  generator  with  mean, 
AM,  and  standard  deviation,  BI. 


2-60 


2.3.5 


LASER  GYRO  MODULE  (GYROS) 


0-)  General  Description 

The  laser  gyro  module  simulates  three  body  mounted  laser  gyros. 

The  laser  gyro  module  contains  the  following  error  sources:  * 

• gyro  input  axis  misalignment 

• scale  factor  error 

• scale  factor  turn-on  transient 

• gyro  bias 

• turn-on  transient  drift 

• angular  random  walk 

• white  angular  noise 

• angular  quantization 

Although  the  user  may  select  alternate  gyro  parameters  by  modifying  the 
gyro  module  initialization  file  (IFILE) , the  nominal  (default)  values 
are  representative  of  the  Honeywell  GG1300  laser  gyro. 

(2)  Gyro  Module  Flow  Diagram 

Figure  1 is  a flow  diagram  of  the  laser  gyro  module.  The  module 
is  initialized  on  the  first  pass  through  the  routine.  The  laser  gyro 
module  initialization  data  file  (IFILE)  is  read  at  initialization  and  the 
input  data  is  converted  into  internal  program  units.  The  program  then 
performs  the  normal  module  operations.  An  analytical  description  of  the 
laser  gyro  module  equations  is  given  in  Volume  II,  Section  5.2.2. 

(3)  Input 

(a)  Module  Initialization  File  (IFILE) 

FORTRAN  unit  numbers  40 
FORTRAN  format:  15,  F20.10 
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r 


i 

¥ 
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Default 

Index 

Variable 

Value 

Units 

Description 

1 

DT 

0.01 

s 

module  operating  time  step 

2 

PRNTSW 

1.0 

logical 

point  switch  (0  - no  print) 
otherwise  print) 

3 

OUTSW 

0.0 

— 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for  printout 

5 

SPAREl 

0.0 

— 

not  used 

6 

SPARE 2 

0.0 

— 

not  used 

7 

MODPDT 

6.0 

s 

module  print  interval 

8 

DB  (1) 

Oo  7 

deg/hr  \ 

9 

DB  ( 2 ) 

0.4 

deg/hr  J 

gyro  bias 

10 

DB  (3) 

-0.6 

deg/hr  J 

11 

DTA(l) 

0.06 

deg/hr  | 

12 

DTA ( 2 ) 

-Oo  03 

deg/hr  / 

transient  drift  amplitude 

13 

DTA (3) 

-0.05 

deg/hr 

14 

DTC(l) 

45.0 

min  ) 

15 

DTC ( 2 ) 

45.0 

( 

min  i 

transient  drift  time  constant 

16 

DTC (3) 

45.0 

min 

1 

17 

SFTA(l) 

0.8 

ppm  j 

| 

18 

SFTA(2) 

-0.7 

PPm 

transient  scale  factor  amplitude 

19 

SFTA ( 3 ) 

1.0 

PPm 

1 

20 

SFTC(l) 

45.0 

min  | 

■ 

21 

SFTC ( 2 ) 

45.0 

min  } 

transient  scale  factor  time  constant 

22 

SFTC ( 3 ) 

45o0 

min  > 

1 

23 

KW  (1) 

1.0 

ppm  > 

24 

KW  (2) 

20.0 

ppm 

gyro  SF  and  gyro  Input  Axis  (IA) 

25 

KW  (3) 

50.0 

ppm 

misalignment  matrix 

26 

KW  (4) 

40.0 

ppm 

• main  diagonal  = SF  bias  (ppm) 
l • off  diagonal  = input  axis 

27 

KW  (5) 

10  5 

ppm 

/ misalignment  (microrad;  ppm) 

28 

KW  (6) 

30.0 

ppm  j 

1 

29 

KW  ( 7 ) 

-30.0 

ppm 

1 

30 

KW  (8) 

o 

. 

0 

1 

ppm 

1 

31 

KW  (9) 

-1. 

ppm  ‘ 

' 

32 

STDWN 

0.0 

sec 

angular  white  noise  std 

33 

STDRW 

0.002 

deg/v4ir 

random  walk  magnitude 

34 

Q 

1.6 

arcsec 

angular  quantization 
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(b) 


Common  Initialization  File  (PFILE) 
FORTRAN  unit  number:  7 
FORTRAN  format:  I5f  F20.10 


Index 

Variable 

Default  Value 

Units 

Description 

1 

WE 

0. 729211 5147E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 

PBUF (1) 

0.0 

— 

not  used 

(4) 

Call-Line 

Data 

Formulation 


(a)  Initialization  Function 

The  switch  INITSW  is  initialized  to  zero  in  DATA  statement.  The 
following  functions  are  performed  on  the  first  pass,  if  INITSW  = 0 and 
IENDF  = 0 and  the  module  operating  cycle  time  has  elapsed. 

• Read  and  print . initialization  file  (IFILE) . 

• Read  common  initialization  fiel  (PFILE) . 

• Set  OFILE  = XFILE. 

• Convert  the  initialization  data  to  internal  program  units.  In- 
ternal units  are:  radians,  feet,  and  seconds. 

DB  = DB  * DTR/3600 
DTA  = DTA  * DTR/3600 

DTC  = DTC  * 60 

SFTA  = SFTA  * l„E-6 

SFTC  = SFTC  * 60 

* * 

KW  = KW  * l.E-6 

Q -■  Q * DTR/3600 

where  DTR  equals  degrees-to-radians  conversion  factor. 

• Compute  the  random  walk  amplitude  coefficient  KRW. 

KRW  = ( STDRW  * DTR  ) * SORT  ( DT/3600  ) 

• Compute  the  exponential  drift  and  exponential  scale  factor  coef- 
ficients . 

EXPD  = EXP(-DT/D¥C  ) 

EXPSF  = EXP (-DT/SFTC  ) 

The  vector  DfC  contains  the  time  constants  of  the  exponential  drifts  and  SFTC 
contains  the  time  constants  of  the  exponential  scale  factors.  The  elements  of 
eJTPd  and  E^SF  are  set  to  zero  if  the  corresponding  time  constants  are  non- 
positive. The  difference  equation  for  each  of  the  terms  is  of  the  form 
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(n+1)  - D^n)  * EXPD 
ST(n+l)  = ST(n)  * EXPSF 

Thus,  the  individual  transients  are  set  to  zero  if  the  associated  time 
constant  is  not  positive.  This  logic  allows  the  user  to  delete  any  of 
the  transient  terms  by  setting  the  amplitude  and  time  constant  to  zero. 

Initialization  is  now  complete,  INITSW  is  set  to  1,  and  the  simula- 
tion time  is  incremented. 

% 

TGYR  = T + DT 

New,  the  routine  returns  to  the  main  program 

(b)  General  Functions 

The  following  functions  are  performed  every  module  operating  cycle. 

• Compute  the  transient  drift  and  scale  factor. 

DTA  = DTA  * EXPD 

SFTA  = SFTA  * EXPSF 

The  transient  terms  are  set  to  zero  if  the  magnitude  becomes  less 
than  l.E-10  to  prevent  underflow. 

• Add  the  scale  factor  transient  to  the  scale  factor  bias. 

Insert  the  sum  on  the  main  diagonal  of  the  rate  sensitive 

* 

matrix  KW. 

KW (K)  = SF ( I ) + SFTA ( I ) , where  K = diagonal  elements. 

* 

The  off  diagonal  elements  of  XW  contain  the  IA  misalignment. 

• Compute  the  white  noise  and  angular  random  walk. 
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White  Angle  Noise 


where 


where 


where 


ANGWN(I)  = STDWN  * GAUSS(0.0,  1.0) 

STDWN  = specified  white  noise  standard  deviation 

GAUSS{0.0,  1.0)  zero  mean,  unit  variance  Gaussian  random  number 


Angular  Random  Walk 

ANGRW(I)  = KRW  * GAUSS  (0.0,  1.0)  • 

KRW  = random  walk  amplitude  coefficient  computed 
in  the  initialization  section 
GAUSS (0.0,  1.0)  = zero  mean,  unit  variance  Gaussian 

randomm  number 

• Compute  the  gyro  output  angular  velocity. 

W « ( I + KW  ) WBB  + DB  + DTA 

WBB  *=  angular  velocity  of  the  body  in  body  coordinates 

DB  = gyro  bias 

DTA  = gyro  drift  transient 

it 

KW  =>  angular  rate  sensitive  matrix 

I = identity  matrix 

• Compute  the  continuous  output  notation  angle  over  the  simulation 
time  step  including  the  white  and  random  walk  angular  noise. 

ANG  = ANG  + W * DT.+  ANGWN  + ANGRW 
The  previous  value  of  the  angle  ANG  is  the  quantization  residual. 

6 Compute  the  quantized  indicated  rotation  over  the  simulation  time 
step,  QANG,  and  the  new  angle  residual,  ANG. 
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NP  = Integer  ( ANG/Q  ) 

QANG  = NP  * Q 

ANG  = ANG  - QANG  - ANGWH 

The  quantized  rotation  QANG  is  set  equal  to  the  continuous  rotation 
angle  if  the  quantization  amplitude  is  zero. 

• Sum  the  quantized  rotations  to  form  the  incremental  gyro  output 
angle  DTHETA.  The  output  angle  is  accumulated  until  the  platform 
attitude  matrix  is  update  - then  the  navigation  routines  reset 
DTHETA  to  zero. 

DTHETA  = DTHETA  _ QANG 

• The  DTHETA  is  printed 

• The  simulation  time  is  incremented 

TGYR  = T + DT 

and  the  subroutine  returns  to  the  main  program. 

(6)  Output 

* 

(a)  Print 

FORTRAN  unit  number:  OFILE  = 6 

On  the  initialization  pass  the  title  "LASER  GYRO  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  > 1.  See  Section  2.2  for  print  control  logic.  The  printed 
output  is,  as  follows 


2-6B 


(7)  Subroutines  Called  (see  Section  2.3.15) 


MXV  = matrix  by  vector  product. 

GAUSS (0.0,  1.0)  = zero  mean,  unit  variance  Gaussian  random 

number  generator 


2.3.6  ACCELEROMETER  MODULE  (ACCEL) 

(1)  General  Description 

The  ACCEL  module  simulates  the  dynamics  of  a pendulous  SDF 
floated  accelerometer.  The  kinematics  of  the  instrument  can  be 
selectively  modeled  using  either  a second-order  or  a first-order 
differential  equation,  or  a "performance"  model.  The  following 
accelerometer  characteristics  are  modeled: 

• Acceleration  sensitivities  (both  g and  g-squared) . 

• Anisoinertia. 

• Output- axis  coupling. 

• Scale-factor  errors  (both  positive  and  negative) . 

• Scale-factor-error  acceleration  sensitivity  (both  positive 
and  negative) . 

• Lever-arm  effects  (including  accompanying  bias) . 

• Quantization  level. 

• Exponentially  correlated  random  bias. 

Although  the  parameters  characterizing  these  effects  are  entirely 
selectable,  th<n  nominal  (default)  parameters  specified  in  the  initiali- 
zation data  file  correspond  to  the  Systron  Donner  4841  accelerometer 
with  its  analog  rebalance  loop. 

(2)  Accelerometer  Module  Flow  Diagram 

The  general  flow  logic  of  the  ACCEL  module  is  illustrated  in 
Figure  1. 

(3)  Input 

(a)  Module  Initialization  File  (IFILE) 

FORTRAN  unit  number:  50 
FORTRAN  format:  15,  IX,  F20.10 
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Figure  1.  Accelerometer  module.  (Sheet  1 of  2) 
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Figure  1.  Accelerometer  module.  (Sheet  2 of  2) 
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dex 

Variable 

Default  Value 

Units 

Description 

1 

DT 

0.01 

s 

module  operating  cycle  time 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print, 
otherwise  print) 

3 

OUTSW 

0.0 

— 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for  printout 

5 

SPARE1 

0.0 

— 

not  used 

6 

SPARE  2 

0.0 

— 

not  used 

7 

CO 

25000. 

dyn-cm/  output-axis  damping  coefficients 

rad/s 

R—  1 *) 

not  used 

O 1^ 

13 

QBAX(l) 

1.0 

unity  \ 

14 

QBAX ( 2 ) 

0.0 

unity  ! 

15 

QBAX (3) 

0.0 

| 

unity  1 

16 

QBAX (4) 

0.0 

unity 

transformation  matrix  from  body 

17 

QBAX (5) 

0.0 

unity 

coordinates  to  X-accelerometer 
) (input,  output,  pendulous) 

18 

QBAX (6) 

1.0 

unity  I 

coordinates 

19 

QBAX (7) 

0.0 

unity  ' 

1 

20 

QBAX (8) 

-1.0 

unity 

| 

21 

QBAX (9) 

0.0 

unity 

/ 

22 

QBAY(l) 

0.0 

unity  \ 

23 

QB AY ( 2 ) 

1.0 

unity  J 

transformation  matrix  from  body 

24 

QBAY (3) 

0.0 

unity  \ 

coordinates  to  Y-accelerometer 
(input,  output,  pendulous) 

25 

QBAY ( 4 ) 

1.0 

unity 

| coordinates 

26 

QBAY (5) 

0.0 

unity 
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In- 

dex 

Variable 

Default  Value 

Units 

27 

QBAY (6) 

0,0 

unity 

28 

QBAY (7) 

0.0 

unity 

29 

QBAY (8) 

0.0 

unity 

30 

QBAY (9) 

-1.0 

unity 

31 

QBAZ(l) 

0.0 

unity  l 

32 

QB AZ ( 2 ) 

0.0 

unity  I 

33 

QBAZ ( 3 ) 

1.0 

unity  1 

34 

QBAZ (4) 

1.0 

unity  [ 

35 

QBAZ (5) 

0.0 

unity  \ 

36 

QBAZ  (6) 

0.0 

unity  I 

37 

QBAZ (7) 

0.0 

unity  1 

38 

QBAZ (8) 

1.0 

unity  1 

39 

QBAZ (9) 

0.0 

unity  1 

40 

MRC 

0.7 

gm-cm 

41 

— 

— 

— 

42 

QUANT 

1.0 

ft/s/ 

pulse 

43 

BIAS (1) 

900.0 

9 

44 

BIAS (2) 

900.0 

g 

45 

BIAS (3) 

900.0 

g 

46 

K 

100000000. 

2 

gm-cm  / 

s 

2 

47 

I 

5.0 

gm-cm 

2 

48 

DELI 

0.0 

gm-cm 

Description 


transformation  matrix  from  body 
coordinates  to  Z-accelerometer 
(input,  output,  pendulous) 
coordinates 


pendulosity  parameter 
not  used 

quantization  level  of  incre- 
ment change  in  velocity 

X ) 

l accelerometer 
( bias 

Z ) 

elastic  restraint  of  rebalance 
loop 

output-axis  moment  of  inertia 

difference  of  moment  of  inertia 
between  pendulous  and  input  axes 
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In- 

dex 

variable 

Default  Value 

Units  Descripti 

49-50 

51 

K0(1) 

0.0 

ug/g  \ 

coefficient  of 

52 

K0(2) 

0.0 

u g/g  > 

output- axis 
acceleration 

53 

K0(3) 

0.0 

ug/g  ) 

sensitivity 

54 

KP<1) 

0.0 

ug/g  \ 

coefficient  of 

55 

KP  (2) 

0.0 

ug/g  ) 

> output-axis 
acceleration 

56 

KP  (3) 

0.0 

ug/g  ) 

| sensitivity 

57 

KII(l) 

0.8 

ug/g2\ 

| coefficients  of 

input- axis 

58 

KII (2) 

0.8 

ug/g  [ 

> acceleration 
squared 

59 

KII (3) 

0.8 

ug/g/ 

sensitivity 

60 

KPP(l) 

0.0 

ug/g2 

2 

J coefficients  of 

/ pendulous-axis 

61 

KPP ( 2 ) 

0.0 

ug/g 

2 

/ acceleration- 
\ squared 

62 

KPP (3) 

0.0 

ug/g 

' sensitivity 

63 

KIO(l) 

0.0 

ug/g2  ' 
2 i 

V coefficient  of 

1 input  by  output- 

64 

KIO (2) 

0.0 

ug/g 

i 

2 

> axis 

l acceleration 

65 

KIO(3) 

0.0 

ug/g  , 

t sensitivity 

66 

KIP (1) 

0.0 

ug/g 

2 ' 

k coefficient  of 

[ input-by 

67 

KIP (2) 

0.0 

ug/g  | 
2 

f pendulous-axis 

1 acceleration 

68 

KIP (3) 

0.0 

ug/g  y 

sensitivity 

69 

KOP(l) 

0.0 

ug/g2 ' 
2 | 

| coefficient  of 

f output-by 

70 

KOP ( 2 ) 

0.0 

U9/92| 

* pendulous-axis 
acceleration 

71 

KOP ( 3) 

0.0 

ug/g  > 

sensitivity 

accelerometer 


Y / accelerometer 

Z ) 

*) 

Y \ accelerometer 

Z ) 

) 

accelerometer 


accelerometer 


acce lerome ter 


accelerometer 
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In- 

dex 


Variable 


Default  Value 


Units 


Description 


72 

BIASV(l) 

0.0 

73 

BIASV(2) 

0.0 

74 

BIASV(3) 

0.0 

75 

SFPO(l) 

10.0 

76 

SFPO (2) 

10.0 

77 

SFP0(3) 

10.0 

78 

SFMO(l) 

10.0 

79 

SFM0(2) 

10.0 

80 

SFMO  ( 3 ) 

10.0 

exponentially 
correlated 
random  bias 
variances 
for 


positive 
scale-factor 
errors  for 


accelerometer 


accelerometer 


negative 
scale-factor 
errors  for 


accelerometer 


81  MODPDT 


6.0 


s 


module  print  interval 


82  ORDER 


0.0 


logical  order  of  differential  equation  model 


83 

SFP  1(1) 

0.0 

ppm/ 

rad/s 

84 

SFP  1(2) 

0.0 

ppm/ 

rad/s 

85 

SFP  1(3) 

0.0 

ppm/ 

rad/s 

86 

SFMl(l) 

0.0 

ppm/ 

rad/s 

87 

SFM1(2) 

0.0 

ppm/ 

rad/s 

88 

SFM1(3) 

0.0 

ppm/ 

rad/s 

0 = performance  model 

1 = first-order  diff.  equation 

2 = second-order  diff.  equation 


positive  rate 
sensitivity  of 
scale- factor 
errors 


Y > accelerometer 


negative  rate 
sensitivity  of 
scale- factor 
errors 


Y > accelerometer 
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In- 

dex 

Variables 

Default  Value 

Units 

Description 

89 

RX(1) 

1.0 

ft 

\ 

position  vector  in  body 

90 

RX(2) 

0.0 

ft 

( 

coordinates  (vehicle  eg 
to  X-accelerometer 

91 

RX(3) 

0.0 

ft 

) 

origin) 

92 

RY(1) 

0.9 

ft 

\ 

position  vector  in  body 

93 

RY  (2) 

0.2 

ft 

( 

coordinates  (vehicle  eg 
to  Y-accelerometer 

94 

RY  ( 3) 

0.0 

ft 

) 

origin) 

95 

RZ(1) 

0.9 

ft 

position  vector  in  body 

96 

RZ  (2) 

0.0 

ft 

coordinates  (vehicle  eg 
to  Z-accelerometer 

97 

RZ(3) 

0.2 

ft 

) 

origin) 

(b)  Common 

Initialization 

File  (PFILE) 

FORTRAN  unit  number: 

7 

FORTRAN  format:  15, 

IX, 

F20.10 

Index 

Variable 

Default  Value 

Units 

Description 

1 

WE 

0.7292115 14 7E- 

4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  cycle  time 

5 

PBUF(l) 

0.0 

— 

not  used 

(4)  Call-Line  Data 


Input 

Tt,  IENDF,  AFb,  WBB , WDO'f', 


DV 

Output 
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(a)  Call-Line  Input 


Variable 

Units 

Data  Type 

Description 

T 

s 

REAL 

current  simulation  time 

IENDF 

logical 

INTEGER 

last-pass  indicator 

ABB 

ft/s2 

REAL 

specific- force  vector  in  body 
frame  with  vibration 

WBB 

rad/s 

n 

REAL 

angular  rate  vector  of  body  wrt 
inertial  space  in  body  frame 
with  vibration  included 

WDOT 

rad/s 

REAL 

angular  acceleration  vector  of  body 
wrt  inertial  space  body  frame  with 
vibration  included 

(b) 

Call-Line  Outi 

5Ut 

Variable 

Units 

Data  Type 

Description 

DV 

ft/s 

REAL 

quantized  integral  of  the  vehicle 

specific-force  vector  along  the 
X,  Y,  Z accelerometer  input  axes 


(5)  Formulation 

(a)  Initialization  Function 

The  initialization  switch,  INITSW,  is  initialized  to  zero  in  a DATA 
statement.  The  following  functions  are  performed  on  the  first  pass,  if 
INITSW  = 0,  IENDF  = 0,  and  if  the  module  operating  cycle  time  has  elapsed. 

• Read  and  print  initialization  file  (IFILE) . 

• Read  common  initialization  file  (PFILE) . 

• Set  OFILE  = XFILE  and  NORDER  = ORDER 

• Convert  initialization  data  to  computational  units. 


SPO 

= SFGPO  * 

l.E  - 6 

sFi 

= SFGPl  * 

l.E  - 6/g 
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SMO 


SFGMO  * l.E  - 6 


SMI  = SFGM1  * l.E  - 6/g 

AQUANT  = QUANT  * MRC/K/30 . 48/DT 
EX  = EXP (-DT/ 40) 

BIASA  = (BIASV  * (1  - EX  ))  ' * l.E  - 6 * G 

BIAS  = BIAS  * l.E  - 6 * G 

In  addition  to  conversion  to  computational  units,  the  following 
variables  are  tested  for  a zero  value  and  the  computation  bypass  switches 
Kll  and  K12,  which  are  initialized  to  zero  in  the  DATA  statement,  are 
reset 

if  BIASV  / 0,  set  Kll  = 1 

Set 


KO 

= 

KO*l.E  - 

6 and  if 

KO 

/ 

o, 

set 

K12 

= 

1, 

KP 

= 

KP*1.E  - 

6 and  if 

KP 

/ 

o, 

set 

K12 

= 

1, 

KII 

= 

k:i*i.e 

- 6/G  and 

if 

KII 

/ 

0, 

set 

K12 

= 

If 

kTo 

a 

KI0*1.E 

- 6/G  and 

if 

KIO 

/ 

0, 

set 

K12 

= 

I, 

kYp 

= 

kip*i.e 

- 6/G  and 

if 

kTp 

/ 

0, 

set 

K12 

= 

If 

KOP 

= 

KOP* l.E 

- 6/G  and 

if 

KOP 

/ 

0, 

set 

K12 

= 

1, 

KPP 

= 

KPP* l.E 

- 6/G  and 

if 

KPP 

/ 

0, 

set 

K12 

= 

I, 

where  l.E  - 6 is  the  factor  for  converting  jjgs  and  G is  the  nominal 
gravity. 
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i 


Continuing, 


MRC  = MRC*7.23E  - 5 

CO  = CO*2.37E  - 6 

DELI  = DELI*2 . 37E  - 6 

K = K*2 . 37E  - 6 

I = 1*2. 37E  - 6 

DTI  = DT/I 

2 

The  factor  2.37E  - 6 is  the  conversion  factor  from  gm-cm  to 

2 

lb-ft  and  the  factor  7.23E  - 5 is  for  conversion  of  gm-cm  to  lb-ft. 

Now  set  INITSVJ  = 1,  increment  the  simulation  time 

TACO  = T + DT 

and  return  the  subroutine  to  the  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating 
cycle.  The  index  K10  is  incremented,  K10  = K10  + 1 from  an  initial 
value  of  zero. 

The  quantized  integral  of  specific  force  vector 
DV,  for  the  X,  Y,  Z accelerometer  is  calculated  in  turn. 

First  for  the  X-accelerometer  (when  II  = 1)  acceleration  from  the 
le**er-arm  effect  is  calculated  from  the  components  of  angular  rate  and 
tha  accelerometer  position  vector.  This  acceleration  plus  the  input 
acceleration  become  the  total  acceleration  sensed  by  the  X accelerometer. 

ABC  =>  ABB  +(WBB  X ( WBB  x RX))+  WDOT  X RX 
input 

lever  arm 
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where  in  the  mnemonics-  of  the  program,  initially 


’ CR0SS1  = WBB  x RX 


and 


CROSS 2 » WBB  x CROSS1 


and  now  redefining 

CROSS!  =•  WDOT  x RX 

The  coordinate!?  of  the  specific  force,  angular  rate,  and  angular 
acceleration  are  transformed  from  (XYZ)  body  coordinates  to  (XOP)  accelerom- 
eter coordinates 


ABA 

= QBAX* 

ABC 

■ 

* 

WBA 

» QBAX  * 

WBB 

* 

WDDTA 

a QBAX  * 

WBCT 

Similarity,  for  the  V-aocolerometer  the  sensed  acceleration  is 
calculated 


ABC  “ ABB  4 (WBB  x (WBB  X RY>)  + WDOT  * R¥ 

& N— zzz — ' 

and  the  specific  force,  angular  rate,  and  angular  acceleration  are 
transformed  to  (10P)  coordinates 


ABA 

• 

» QBAY 

* ABC 

WBA 

- QBAX 

* WBB 

WDOTA 

“ QBAX 

* WDOT 

Finally,  for  the  Z accelerometer 


and 


ABC  - ABB  + (W3B  x (WBB  X RZ) )+  WDOT  x RZ 
input  lever  arm 


ABA 

® QBAZ 

* ABC 

WBA 

- QBAZ 

* WBB 

* 

WDOTA 

=>  QBAZ 

* WDOT 

■ Now  for  accelerometers  X,  Y,  and  Z,  when  II  = 1,  2,  and  3, 
respectively,  the  following  terms  are  calculated. 


If,  K10  » 1,  which  occurs  on  the  first-pass  filter  after  initial!-, 
zaticn.  then  set 


THETA (II)  » (MRC/K)  * A3A(1) 
and  WDOTA(2)  « 0.0 


The  output  axis  value  bias  levels  are  now  calculated,  if  the  bias 
variance  is  zero,  Kll  » 0,  and  the  random  bias  is  set  to  the  bias  level, 

BXA  « BIAS  (ID 


Otherwise,  a Gaussian  random  number  with  zero  mean  and  standard  deviation, 

BX,  where  BX  m BXASA(Il)  is  generated  and  the  exponentially  correlated  random 
bias,  BIA,  is  calculated,  as  follows.  The  value  of  ABIAS  for  the  Particular 
accelerometer,  II,  from  the  previous  pass  (on  the  first  pass  after  the 
initialization  pass,  ABIAS  ■ (0.0,  0.0,  0.0))  is  multiplied  by  the  exponential 
function,  EX,  and  added  to  the  random  number  from  the  GAUSS  function. 

ABIAS  (II)  » ABIAS  (II)  * EX  + GAUSS  (AM, BD 


where 


EX  - EXP(-DT/40) . 
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Finally,  the  exponentially  correlated  random  bias  for  the  II 
accelerometer  is 


BIA  = BIAS (II)  + ABIAS(I)) 

The  dynamic  equations  of  the  pendulous  single-degree-of-freedom 
floated  accelerometer  are  solved  for  the  output-axis  float  offset  angle, 
THN , for  any  one  of  three  approximate  forms: 

Performance  model,  when  NORDER  = 0. 

First-order  model,  when  NORDER  = 1. 

Second-order  model,  when  NORDER  = 2. 

See  Volume  II,  Section  6 for  a theoretical  development  of  the  dynamic 
equations . 

A quantity  DEN  is  defined 

DEN  * K + DEU*(WBA(3)2  - WBA(l)2)  - MRC*ABH(3) 

where  the  subscripts  are 

1 a input  axis 

2 « output  axis 

3 » pendulous  axis 

The  quantity  DEN  times  the  float  offset  angle  is  a torque  term. 

An  additional  torque  term,  Ml,  is  defined  as 

Ml  « MRC* (ABA(l)  - BIA)  + DELI  * WBA(3) *WBA(1)  - I*WDOTA(2) 

Additional  torque  terms  in  the  numerator,  denoted  by  M2,  are  com- 
puted for  acceleration  and  acceleration-squared  sensitive  effects  if  any 
of  the  coefficients  are  nonzero,  i.o.,  when  K12  = 1. 
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M2  = MRC*(~KP(I1)*ABA(3)  - KO(Il) *ABA(2) 

- KPP(I1)*ABA(3)*ABA(3)  - KII (II) *ABA(1) *ABA(1) 

- KIP(Il) *ABA(1) *ABA(3)  - KIO (11) *ABA(1) *ABA(2) 

- KOP(Il) *ABA(2) *ABA(3) ) 

Otherwise,  M2  - 0.0 

If  norder  = 0,  the  performance  model  equation  is  solved  for  the 
float  offset  angle,  THN 

THN  = (Ml  + M2) /DEN 

If  NORDER  = 1,  the  first-order  model  equation  is  solved  for  THN. 

THN  = THETA (II)  + DT* (-DEN*THETA(I1)  + Ml  + M2) /CO 

Finally,  if  NORDER  = 2,  the  second-order  model  equation  is  solved  for 
THN 


THN  - THETA  (II)  + DT*THD0T(U) 

where  THDOT(Il)  is  calculated  on  the  previous  pass 
Now  THDOT(Xl)  is  calculated  for  the  next  pass  from 

THDOT(Il)  « THDOT(Il)  + (-C0*THD0T(I1)  - DEN*THETA(Xl>  + Ml  + M2)*DTI 

The  quantized  output  is  now  calculated.  The  float  offset  angle, 
THN,  is  added  to  the  residual  angle,  TTHET,  from  the  previous  pass 


TTHET(Il) 


TTHET  (II)  4-  THN 


Scale- factor  error  corrections  are  computed  for  positive  accelera- 
tions if  the  residual  angle,  TTHET(ll) , is  greater  than  or  equal  to  zero 
from 


T1  = SPO(Il)  + 1 + SPl(Il) *THN*K/MRC 
or  if  it  is  less  than  zero  for  negative  acceleration  from 
T1  - SMO(Il)  + 1 + SMl(Il) *PHN*K/MRC 

The  sum  of  the  float  offset  angle  and  the  residual  angle  are 
corrected  for  the  scale-factor  errors  by  multiplying  by  Tl,  and  this 
quantity  is  divided  by  the  quantization  level,  AQUANT.  The  truncation 
of  this  quantity 


XTHET  ■ TTHET(Il) /AQUANT/TI 

results  in  the  quantized  float  offset  angle,  i.e.  the  count  of  quantization 
pulses  contained  in  it, 

A new  residual  angle,  i,o«  the  portion  dropped  off  at  truncation, 
is  calculated  from 

TT  - FLOAT (ITNET) 

and  TTHBT(Xl)  - TTHET(Il)  - TP*TX*AQUANT 
and  the  float  offset  angle  is  saved  for  the  calculations  of  the  next  pass 

THETA(Il)  « THN 

Now,  the  dosired  output  from  the  ACCEL  module,  DV,  is  obtained  for  the 
X,  Y,  and  2 accelerometers  (II  » 1,  2,  or  3,  respectively)  by  integrating 

DV(X1)  - DV(Il)  + TT#AQUANT/MRC*K*DT 


where 

VS  m FLOAT (XTHET)  (maintains  real  variable  arithmetic) 
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This  summation  is  carried  out  to  equalize  the  cycle  intervals  for  the 
hardware  and  software.  DV  is  initialized  in  the  RDR  module  (the 
hardware-software  interface)  each  RDR  cycle. 

The  quantized  integrated  change  in  velocity  is  printed,  the 
simulation  time  is  incremented 

TACC  » T + DT 

and  the  subroutine  returns  to  the  main  program. 

(6)  Output 

(a)  Print 

FORTRAN  unit  numbers  OFXLS  =•  6 

On  the  initialization  pass  the  title  "ACCELEROMETER  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  1.  See  Section  2.2  for  print  control  logic.  The  printed 


output  is,  as 

follows 

Variable 

Units 

Description 

DV 

ft/s 

quantized  integral  of  specific  force 
the  X,  V,  and  Z accelerometer  input  axes 
over  the  computation  cycle  plus  errors 

(7)  Subroutines  Called  (See  Section  2.3.15) 
HXV  - matrix  by  vector  product 
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2.3.7  ALTIMETER  MODULE  (ALTI) 


(1)  General  Description 

The  ALTI  module  is  designed  to  simulate  the  output  characteristics 
of  a barometric  altimeter,  incorporating  errors  in  its  air-transducer 
assembly  and  uncertainties  caused  by  nonstandard-atmosphere  and  Pitot- 
static  errors.  The  integration  of  this  simulated  instrument  into  the 
INSS  gives  rise  to  an  indicated  altitude  which  has  the  following  char- 
acteristics s 

• Long-term  stability  of  the  barometric  altimeter. 

• A resultant  frequency  response  that  is  much  faster  than  that 
of  a pure  barometric  altimeter,  and  which  attenuates  the  ef- 
fects of  barometric  noise. 

• The  relative  insensitivity  to  low-frequency  accelerometer 
errors  resolved  into  tho  vertical  channel. 

Essentially,  the  indicated  altimeter  output  is  the  sum  of  the 
"true"  vehicle  altitude  (passed  from  the  trajectory  module)  and  user- 
characterizad  exponentially  correlated  random  bias  and  random  noise. 
Although  a perfect  altimeter  can  bo  simulated  by  this  modulo  through 
user-specification  of  an  on/off  switch  (N0ISSW»1)  in  the  attendant 
initialization  data  file,  the  nominal  (default)  initialization  parameters 
correspond  to  tho  instrument  projected  for  use  on  the  Space  Shuttle. 

(2)  Altimeter  Module  Flow  Diagram 

The  general  flow  logic  of  the  ALT!  module  is  shown  in  Figure  1. 

(3)  Input 

(a)  Module  Initialization  File  (XFILE) 

FORTRAN  unit  number:  60 
FORTRAN  formats  15,  F20.10 
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In- 

dex 

1 

2 

3 

4 

5 

6 
7 


Variable  Default  Value 


DT 

PRNTSW 

OUTSW 

XFILE 

MQDPDT 

NOISSW 

TC 


8 UO 

9 Ul 

10  U2 

11  03 

12  04 


0.02 

1.0 

0.0 

6.0 

6.0 

0.0 

40.0 


97.0 


Units 

s 

logical 


logical 

s 

logical 

s 


0.000000000000040  1/ft 

0.000000023 

17000. 

0.000000000625 


Description 

module  operating  frequency 

print  switch  (0  - no  print, 
otherwise  pring) 

not  used 

FORTRAN  unit  number  for 
printout 

module  print  interval 

noise  generator  switch 

exponentially  correlated 
random  noise  time  constant 

.altitude  to  bias 
uncertainty 


ft 


2 


ft' 


vari- 
ance 

coeffi- 

cients 

relat- 

ing 


velocity  to  bias 
uncertainty 

bias  uncertainty 
constant 

altitude  to  random 
bias  uncertainty 
random-vibration 

‘uncertainty  (constant 
random  vibration) 


(b)  Common  Initialisation  File  (PFILE) 
FORTRAN  unit  number:  7 
FORTRAN  format:  15,  F20.10 


In- 

dex 

Variable 

Default  Value 

Units 

Description 

1 

WE 

0.72921I5147E-4 

rad/s 

cartli  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

prinHng  frequency 

5 

PBUF(l) 

0.0 

not  used 
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(4)  Call-Line  Data 


INPUT  

(T,  IENDF,  ALT,  V 

ALTO 

OUTPUT 

(a)  Call-I»ine  Input 


Variable 

Units 

Data  Type 

Description 

T 

s 

REAL 

current  simulation  time 

IENDF 

logical 

INTEGER 

last-pass  indicator 

ALT 

ft 

REAL 

“true14  altitude  (from  tra- 
jectory module) 

V 

ft/s 

REAL 

"true*  velocity  vector  in  body 
frame  (from  trajectory  module) 

<b) 

Call-Line  Output 

Variable 

Units 

Data  Typo 

Description 

ALTO 

ft 

REAL 

indicated  barometric  altitude 

{Si  formulation 

(a)  Initialization  Function 

Thu  initialization  switch,  JNITSW,  is  initialized  to  zero  in  an 
DATA  statement.  The  following  functions  are  performed  on  the  first 
pass,  if  INITSW  “ 0,  jendf  “ 0 and  if  the  module  operating  cycle  time 
has  elapsed. 
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Read  and  print  initialization  file  (FILE) . Read  common  initiali- 
zation file  (PFILE) . Set 


OFILE 

= 

XFILE 

ALTO 

a 

ALT 

EX 

a 

EXP  l-DT/TC) 

E2X1 

a 

1 - EX2 

INITSW 

a 

1 

The  simulation  time  is  incremented 

TALT  = T + DT 

and  the  subroutine  returns  to  the  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating  cycle. 

If  the  noiso  generator  switch  is,  NOISSW  *»  0,  the  indicated 
altitude,  ALTO,  is  equal  to  the  true  altitude,  ALT.  Otherwise,  the 
barometric  altitude  with  noise  added  is  calculated. 

The  model  chosen  to  replicate  the  output  of  a barometric  altimeter 
is  fundamentally  stochastic.  As  such,  five  statistical  parameters 
(variances)  specified  in  the  attendant  data  file  are  employed  to  gen- 
erate random  errors.  These  errors  are  then  summed  with  the  "true** 
altitude  (ALT)  passed  over  from  the  trajectory  module  to  simulate  an 
output  measurement.  The  five  input  statistics  represent  the  variances 
which  characterize:  bias  uncertainties  relative  to  altitude  (UQ)  anti 
velocity  (u^),  constant  bias  uncertainty  (U^),  random- vibration  uncer- 
tainty relative  to  altitude  (u  , and  constant  random- vibration  uncer- 
tainty (0^) . These  variances  are  subsequently  combined  by  the  algorithm 
to  generate  both  exponentially  correlated  random  bias  and  Gaussian  noise 
over  each  cycle. 


A first  term  (the  random  noise) , a normally  distributed  random 
number  with  a mean,  AM  = 0.0,  and  a standard  deviation  given  by 

2 1/2 

SIGR  = (U3*ALT  + U4)  ' 
is  computed  using  the  GAUSS  function. 

Another  term  (the  random  bais) , a normally  distributed  random 
number  with  a mean,  AM  = 0.0,  and  a standard  deviation  given  by 

SIGN  = [(UO*(ALTA2)2  + Ul*(VI2)2  + U2)  *E2Xl]  1/2 


where 


ALTA 2 - ALT 2 

VI 2 - V(l)2  ♦ V(2) 2 + V(3) 2 

is  obtained  by  the  use  of  the  GAUSS  subroutine.  An  exponentially  cor- 
relat  Hi  noise  term,  X,  is  compute  from  the  random  bias  tern, 

X - EX*X  + GAUSS (AM, SIGN) 

where  x on  the  right-hand  side  of  the  equation  is  the  value  of  X on  the 
previous  pass  (initially  rare). 

The  indicated  barometric  altitude,  ALTO,  is  computed  as  the  sum 
of  the  true  altutude,  the  random  bias  and  the  random  noise  tern 

ALTO  “ ALT  ♦ X ♦ GAUSS (AH,  SIGR) 

The  indicated  altitude,  ALTO,  is  printed  and  the  simulation  time 
is  incremented 

TALT  - T ♦ DT 

New  the  subroutine  returns  to  the  main  program. 
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(6) 


Output 
(a)  Print 


FORTRAN  unit  number:  OFILE  = 6. 

On  the  initialization  pass  the  title  "ALTIMETER  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRINTS w >_  1.  See  Section  2.2  for  print  control  logic.  The  printed 
output  is,  as  follows 

Variable  Units  Description 

ALTO  ft  indicated  barometric  altitude 

(7)  Subroutines  Called  (See  Section  2,3.15) 

GAUSS (AM, SD)  = Gaussian  random  number  generator  function 
with  mean,  AM,  and  standard  deviation,  SD. 


2.3.8  HARDWARE/SOFTWARE  INTERFACE  MODULE  (RDR) 

(1)  General  Description 

Functioning  as  a buffer  between  the  fast  and  slow  cycles  of  the 
INSS  executive  (sequencer) , this  module  effectively  simulates  the  action 
of  the  hardware/software  interface.  After  reading  the  accumulated  in- 
cremental angle  and  velocity  components,  the  interface  module  passes  the 
appropriate  values  to  the  compensation  algorithms.  The  face  values 
previously  accumulated  are  then  reset  to  zero  and  returned  to  the  gyro- 
and  accelerometer-module  accumulation  registers. 

(2)  Hardwaro/Software  Interface  Module  Flow  Diagram ' 

The  general  flow  logic  of  the  RDR  module  is  shown  in  Figure  1. 

(3)  Input 

(a)  Module  Initialization  File  (I FILE) 

FORTRAN  unit  number:  65 
FORTRAN  format:  15,  F20.10 


index 

Variable 

Default  Value 

Units 

Description 

1 

m 

0.02 

s 

modulo  operating  frequency 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print, 
otherwise  print) 

3 

OUTSW 

0.0 

— 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for 
printout 

5 

SPARE 1 

0.0 

— 

not  used 

6 

MODPDT 

6.0 

0 

module  print  interval 
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(b)  Common  Initialization  File  (PFILE) 
FORTRAN  unit  numbers  7 
FORTRAN  format:  15,  F20.10 


Index 

Variable 

Default  Value 

Units 

Description 

1 

WE 

0. 7292115147E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 

PBUF(l) 

0.0 

mmmmt 

not  used 

(4)  Call-Line  Data 


In/Out 


DTHETO , DVO) 
Output 


l!a)  Call-Line  input 


Variable 

Units 

Data  Typ< 

'T 

s 

REAL 

IENDF 

logical 

INTEGER 

DTHETA 

rad 

REAL 

DV 

ft/s 

REAL 

Description 

current  simulation  time 

last-pass  indicator 

quantized  integral  of  angular 
rate  about  X,  Y,  Z gyro  input 
axes  over  the  computation  cycle 
plus  errors 

quantized  integral  of  the  vehicle 
specific  force  vector  along  the 
X,  Y,  Z accelerometer  input  axes 
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(b)  Call-Line  Output 


• • r=wvtfegaw*v*> 


Description 

quantized  integral  of  angular  rate 
about  X,  v,  Z gyro  input  axes  gen- 
erated each  fast  cycle,  and  accumu- 
lated over  the  slow  cycle 

quantized  integral  of  the  vehicle 
specific  force  vector  along  the  X, 

Y,  Z accelerometer  input  axes  gen- 
erated each  fast  cycle,  and  accumu- 
lated over  the  slow  cycle 

(5)  Formulation 

(a)  Initialization  Function 

The  switch,  INITSW,  is  initialized  to  2ero  in  a DATA  statement. 
The  following  functions  are  performed  on  the  first  pass,  if  INITSW  » 0, 
IENDF  >=>  0,  and  if  the  module  operating  cycle  time  has  elapsed, 

• Read  and  print  initialization  file  (IFILE) . 

• Read  common  initialization  file  (PFILE) . 

• Set  OFILE  » XFILE  and  INITSW  =»  1. 

The  simulation  time  is  incremental 

TRDR  *»  T + DT 

and  the  subroutine  returns  to  the  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating 

cycle. 


Variable  Units  Data  Type 
DTHETO  rad  REAL 


DVO 


ft/s 


REAL 
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The  input  variables  DV  and  DTHETA  are  stored  as  the  corresponding 
output  variables,  DVO  and  DTHETO,  then  the  input  variables  are  reset 
to  zero  (and  reLurned  to  the  accelerometer  and  gyro  modules) . 


DVO  = DV 
DV  = 0.0 

DTHETO  = DTHETA 
DTHETA  = 0.0 


The  quantized  integrals  of  angular  rate,  DTHETO,  and  specific 
force,  DVO  are  printed  and  the  simulation  time  is  incremented 

TRDR  o T + DT 

Now  the  subroutine  returns  to  the  main  program. 

(6)  Output 

(a)  Print 

FORTRAN  unit  number:  QF2LE  «*  6 

On  the  initialization  pass  the  title,  "READER  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  > 1.  See  Section  2.2  for  print  control  logic.  The  printout 
is  as  follows 


Variable 

Units 

Description 

Wrtfc  m* 

DVO 

ft/s 

quantized  integral  of  specific  force  along 
the  X,  V,  Z accelerometer  input  axes  accumu- 
lated over  slow  cycle 

DTHETO 

rad 

quantized  integral  of  angular  rate  about  the 
X,  Y,  2 gyro  input  axes  accumulated  over 
slow  cycle 

(7)  Subroutines  Called  (See  Section  2.3.15) 
No  subroutines  are  called. 
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2.3.9  ACCELEROMETER  COMPENSATION  MODULE  (ACOMP) 

(1)  General  Description 

This  software  module  computes  the  compensation  for  accelerometer 

bias,  scale- factor  errors,  scale-factor  errors  with  acceleration,  the 
acceleration-squared  terms  along  the  input  axis,  accelerometer  misalign- 
ments, anisoinertia,  output-axis  coupling,  and  lever-arm  effects. 

(2)  Accelerometer  Compensation  Module  Flow  Diagram 

Figure  1 depicts  the  general  flow  logic  of  the  ACOMP  module. 


(3)  Input 

(a)  Module  Initialization  File  (IFILE) 
FORTRAN  unit  number:  67 
FORTRAN  format:  15,  F20.10 


Index 

Variable 

Default  Value 

Units 

Description 

1 

DT 

0.02 

8 

module  operating  frequency 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print, 
otherwise  print) 

3 

OUTSW 

0.0 

WNW 

not  used 

4 

XFXLE 

6.0 

logical 

FORTRAN  unit  number  for 
printout 

5 

SPARE 1 

0.0 

— 

not  used 

6 

SPARE2 

0.0 

— 

not  used 

7 

ftEjT.T 

0.0 

2 

gja-cm 

difference  of  moments  of 

inertia  about  pendulous  and 
input  axes 
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''MMRMmsmsh 


»«!S  ’■'•’IWlWK'SI 

Index 

Variable 

Default  Value 

Unit 

Description 

8 

QABX(l) 

1.0 

unity  \ 

9 

QABX{2) 

0.0 

unity 

10 

QABX(3) 

0.0 

unity 

11 

QABX(4) 

0.0 

unity  I 

[ 

transformation  matrix  from 

12 

QABX(5) 

0.0 

unity  \ 

body  coordinates 

to  X- 

/ 

accelerometer  (input,  output, 

13 

QABX(6) 

1.0 

unity  j 

pendulous)  coordinates 

14 

QABX(7) 

0.0 

unity  1 

15 

QABX(8) 

-1.0 

unity 

16 

QABX{9) 

0.0 

unity  , 

17 

SFP0(1) 

10,0 

/ 

ppm 

X-accelerometers ) 

| 

18 

SFP0(2) 

10.0 

ppm 

V-accelerometer  * 

' positive 
scale- faotor 

19 

SFP0(3) 

10.0 

ppm 

Z-accelerometer  J 

| error 

20 

BIAS (1) 

890.0 

US 

X-accelerometer  1 

21 

BIAS  (2) 

890.0 

US 

Y-acceleroweter  ; 

• bias 

22 

BIAS (3) 

890.0 

pg  Z-accelerometer  J 

k 

23 

QABY{1) 

0.0 

unity 

. 

24 

fiABY(2) 

1,0 

unity 

25 

QABY(3) 

0.0 

unity 

26 

QABY(4) 

1.0 

unity 

transformation  matrix  from 

27 

QABY(5) 

0.0 

unity 

) body  coordinates 

to  Y- 

j 

accelerometer  (input,  output, 

28 

fiABY(6) 

0.0 

unity  j 

pendulous)  coordinates 

29 

QABY(7) 

0.0 

unity 

30 

GABY<a) 

0.0 

unity 

31 

QABY (9) 

-1.0 

unit  j 
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Index  Variable  Default  Value  Unit 


32 

QABZ(l) 

0.0 

unity 

33 

QABZ(2) 

0.0 

unity 

34 

QABZ (3) 

1.0 

unity 

‘35 

QABZ (4) 

1.0 

unity 

36 

QABZ(5) 

0.0 

unity 

37 

QABZ (6) 

0.0 

unity 

38 

QABZ (7) 

0.0 

unity 

39 

QABZ (8) 

1.0 

unity 

40 

QABZ (9) 

0.0 

unity 

41 

MODPDT 

6.0 

s 

42 

KII(l) 

0.4 

ug/g2 

43 

KII (2) 

0.4 

vg/g2 

44 

KII (3) 

0.4 

ug/g2 

45 

MRC 

0,7 

gm-cm 

46 

QMIS(l) 

1,0 

unity 

47 

QM1S (2) 

0.0 

unity 

48 

QKXS(3) 

0.0 

unity 

49 

QHIS(4) 

0.0 

unity 

50 

QMIS(5) 

1.0 

unity 

51 

QMIS(o) 

0.0 

unity 

52 

QMIS(7) 

0,0 

unity 

53 

QMXS  (8) 

0.0 

unity 

54 

QHIS(9) 

1.0 

unity 

Description 


transformation  matrix  from 
body  coordinates  to  Z- 
accelerometer  (input,  output, 
pendulous)  coordinates 


module  print  interval 


X-accelerometer  \ compensation 
I input  axis  ac- 
Y-aceelerometer  l celeration- 
I squared  coef- 
S-accelerometer  / ficients 


pendulosity  parameter 


compensation  misalignment 
transformation  matrix  from 
X,  Y,  Z accelerometer  output 
axis  to  X,  Y,  2 body  axes 
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■7VO'X>S.»V: 


Index 

Variable 

Default  Value 

Unit 

Description 

55 

SFM0(1) 

10.0 

ppm 

X-accelerometer  \ compensation 

I negative 

56 

SFM0(2) 

10.0 

ppm 

Y-accelerometer  > scale- 
I factor 

57 

SFM0(3.) 

10.0 

ppm 

Z- accelerometer  / error 

58 

SFPl(l) 

0.0 

ppm/g 

X-accelerometer  \ compensation 

I positive  rate 

59 

SFPl(2) 

0.0 

ppm/g 

Y-accelerometer  / sensitivity  of 
1 scale-factor 

60 

SFPl (3) 

0.0 

ppm/g 

Z-accelerometer  / errors 

61 

SFMl(l) 

0.0 

ppm/g 

X-accelerometer  \ compensation 

1 negative  rate 

62 

SFM1{2) 

0.0 

ppm/g 

Y-accelerometer  > sensitivity  of 
1 scale- factor 

63 

SFM1(3) 

0.0 

ppm/g 

Z-accelerometer  ) errors 

% 

64 

RX(1) 

1.01 

ft  ) 

compensation  position  vector 

65 

RX(2) 

0.0 

ft  | 

r (vehicle  eg  to  X-accelerom- 

eter  origin)  in  body  coordinates 

66 

RX  (3) 

0.0 

ft  J 

% 

67 

RY(1) 

0.89 

ft  ] 

f compensation  position  vector 

68 

RY(2) 

0.19 

" ) 

} (vehicle  eg  to  Y-accelerom- 

eter  origin)  in  body  coordinates 

69 

RY(3) 

0.0 

ft  > 

70 

RZ(1) 

0.91 

ft  ' 

1 

compensation  position  vector 

71 

RZ(2) 

0.0 

* 1 

5 (vehicle  eg  to  Z-aocelerom- 
oter  origin)  in  body  coordinates 

72 

R2(3) 

0.19 

ft  i 

73 

m 

5.0 

2 

gm-cm 

moment  of  inertial  about  output 
axis 

(b)  Common 

Initialization 

File  (PFILE) 

FORTRAN  unit  number;  7 


FORTRAN  format!  15,  F20.10 
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Index  Variable 


f'efault  Value 


Unit3 


Description 


| 


l 


1 

WE 

0. 7292115147E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.0 

ft/s2 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 

PBUF(l) 

0.0 

not  used 

(4)  Call-Line  Data 


Injjw- 

(T,  IEMDF,  DVO,  DTHETA, 


Output 


(a)  Call-Line  Input 

Variable  Unite  Data  Type  Description 

T s REAL  current  simulation  time 


XENDP  logical  INTEGER  last-pass  indicator 


DVO  ft/s  REAL 


b 


rad 


REAL 


quantised  integral  of  vehicle 
specific  force  along  the  X,  V,  2 
accelerometer  input  axes  {over 
the  fast  cycle  accumulated  over 
slow  cycle) 

quantised  integral  of  angular  rate 
about  the  X,  Y,  2 accelerometer 
input  axes  (over  the  fast  cycle 
accumulated  over  slow  cycle) 


(b) 


Call- Line  Output 


Variable  Units  Data  Type  Description 

DVA  ft/s  REAL  quantized  integrrl  of  vehicle 

specific  force  along  X,  Y,  Z 
accelerometer  input  axes  (accumu- 
lated and  compensated) 


(5)  Formulation 

(a)  Initialization  Function 

The  initialization  switch,  INITSW,  is  initialized  to  zero  in  a 
DATA  statement.  The  following  functions  are  performed  on  the  first  pass, 
if  INITSW  o 0,  IENDF  ° o,  and  if  the  module  operating  cycle  time  has 
elapsed. 

• Read  and  print  initialization  file  (IFILE). 

• Read  common  initialization  filo  (PFILE) , 

• Set  OFILE  « XFXLE 

Define  tho  matrix,  Qa6o  from  the  second  rows  (A.e. , the  output 
axes  of  the  X,  Y,  and  2 accelerometer)  of  QADX,  QABY,  and  QAB2,  re- 
spectively, 


QABXC4) 

QABX(S) 

QADX(6) 

QABY<4) 

QABY (5) 

QABY (6) 

QAD2(4) 

QAB2 (5) 

QAB2{6) 
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Convert  initialization  data  to  computational  units 


BIAS 

S3 

BIAS  * l.E  - 6*G 

KI1 

= 

KII  * 1.E-6/G 

IXX 

a 

IXX  * 2.37E  - 6 

DELI 

« 

DELI  * 2.37E  - 6 

MRC 

a 

MRC  * 7.23E  - 5 

SPO 

a 

SFPO  * l.E  - 6 

SPl 

a 

SFPl  * l.E  - 6/G 

SMO 

a 

SFMO  * l.E  - 6 

SKI 

a 

SFHl  * l.E  - 6/G 

whero  the  conversion  factors  aro 


lb- ft2 

m ‘ 

2 

2.37E  - 6 gm-cm 

Ib-ffc 

» 

7.23E  - 5 gm-cm 

unity 

ii 

l.E  - 6 ppm 

g ft/s2 

« 

(l.E  - 6*G>  ug 

g ft/s2 

m 

(l.E  - 6/G)  g/g 

The  initialisation  switch  is  sot,  INITSW  1 and  tae  current 
simulation  time  is  incremented 

TCAC  = T +•  DT 

Now  the  subroutine  returns  to  the  main  program. 
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(6) 


General  Function 


The  following  functions  are  performed  every  module  operating  cycle. 
The  switch,  K10,  which  was  initialized  to  0,  is  reset  to  1 upon  comple- 
tion of  the  first  pass  after  initialization. 

The  rate  of  change  of  angular  velocity,  WDOT,  resolved  along  the 
three  accelerometer  output  axes,  is  bypassed  on  the  first  pass  after 
initialization,  but  calculated  on  every  subsequent  pass 

WDOT  = QBAO  (DTHETA  - DTHETP)/DT 

where 

DTHETP  = DTHETA  of  the  previous  pass 

The  quantized  incremental  velocity  (the  quantized  integral  of 
the  specific  force),  DVO,  for  each  of  the  X,  Y,  and  Z accelerometers, 
in  turn,  is  compensated  for  the  effects  of 

• scale-factor  error  (positive  and  negative) . 

• bias. 

• acceleration-squared  sensitivity  along  the  input  axes. 

• anisoinartia. 

• lever-arm  effect. 

• output-axis  couplings. 

Scale- factor  error  is  compensated  by 

DVO  « DVO* ( 1 - SPO  + SP1*DV0/DT) 
if  DVO  >.0.0,  for  positive  scale-factor  error  and  by 
DVO  = DVO* ( 1 - SMO  + SM1*DV0/DT) 
if  DVO  < 0.0,  for  negative  scale-factor  error. 
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'liim  i'. 


Bias  compensation  is  accomplished  by  use  of  the  relation 

DVO  = DVO  + BIAS  * DT 

The  components  of  DVO  correspond  to  the  X,  Y#  Z accelerometer 
input  axes. 

The  cross  products  (CR0SS1  and  CR0SS2)  are  calculated  for  the 
X~accelerometer  to  obtain  the  lever  arm  effect  by 

CROSS 1 = DTHETA  X RX 

CR0SS2  = (DTHETA  X CR0SS1)/DT 

and  DTHETA  is  transformed  from  body  to  X-accelerometer  coordinates 
every  pass 


DTHETZ  = QABX  * DTHETA 

where  the  components  are 

1.  input  axis 

2 . output  axis 

3.  pendulous  axis 

while  che  caculation  of  the  cross  product#  CR0SS3(1)#  for  the  X accel- 
orometor  is  bypassed  only  on  the  first  pass  after  initialisation  and 

CR0SS3  (1)  « (DDTH{2)  *RX(3)  - DDTH(3)  *JU«2) ) /DT 


whore 

DOTH (2)  - DTHETA (2)  - DTHETP(2) 

DOTH (3)  » DTHETA (3)  - DTHETP(3) 
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Similarly,  for  the  Y-accelerometer 


CROSS 1 = DTHETA  x RY 

CROSS 2 = (DTHETA  x CROSSl)/DT 


DTHETZ 


* 


QABY  * DTHETA 


CROSS 3 (2)  « (DDTH(3) *RY(1)  - DDTH(l) *RY (3) )/DT 


DOTH (3)  o DTHETA (3)  - DTHETP(3) 

DDTH(l)  ■»  DTHETA ( 1 ) - DTHETP(l) 

Also,  for  the  Z accelerometer 

CROSS 1 « DTHETA  x RZ 

CR0SS2  « (DTHETA  x CROSSD/DT 

DTHETZ  « QABZ  * DTHETA 


CROSS3 (3)  * (DDTH(l) *R2(2)  - DDTH(2)*RZ(1> )/DT 


DOTH(l)  « DTHETA (1)  - DTHETP ( 1 ) 
DDTIK2)  « DTHETA (2)  - DTHETP  (2) 


The  lever-arm  effect  is  compensated  by  the  terms  CR0SS2  and  CROSS 3 on 
the  incremental  velocity 


DVO  = DVO  - CROSS 3 - CR0SS2 

The  acceleration-squared  sensitivity  along  the  input  axis  is 
compensated  by  the  term 

kTi  * dvo2/dt 

for  the  X,  Y,  and  Z accelerometers  on  every  pass. 

The  anisoinertia  is  compensated  by  the  term 

DELI*DTHETZ ( 3) *DTHETZ (1) / (DT*MRC) 

for  the  X,  V,  and  Z accelerometers  using  the  pendulous  (3)  and  input  (1) 
components  on  every  pass. 

The  acceleration-squared  sensitivity  along  the  input  axis  and 
the  anisoinertia  are  added  to  the  incremental  velocity  for 

DVO  ■=>  DVO  + KII*DV02/DT  + DELI  * DTHETZ  < 3 ) * DTHETZ  { 1 ) / ( DT*MRC) 

l 

along  the  X,  V,  and  Z accelerometer  input  axes. 

Finally,  the  output-axis  coupling  is  compensated  on  all  passes, 
except  the  first  pass  after  initialization  for  the  X,  Y,  and  Z accel- 
erometers, and  added  to  the  incremental  velocity 

DVO  » DVO  + I XX  * WDOT/MRC 
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At  the  end  of  the  first  pass  after  initialization  and  compensa- 
tion of  the  DVO  along  the  x,  Y,  and  Z accelerometer  input  axes,  the 
switch  K10  is  reset,  K10  * 1. 

On  every  pass  the  quantized  integral  at  angular  rate  is  restored 
in  DTHETP  for  use  in  the  next  pass 

DTHETP  » DTHETA 

Finally,  the  quantized  integral  of  vehicle  specific  force  (incremental 
velocity)  along  the  X,  Y,  and  Z accelerometer  input  axes.DVA,  is  com- 
pensated for  accelerometer  misalignment 

DVA  » QMIS  * DVO 

The  compensated  incremental  velocity,  DVA,  is  printed  and  the 
simulation  time  incremented 

TCAC  ■ T + DT 

The  subroutine  now  returns  to  the  main  program. 

(6)  output 

(a)  Print 

FORTRAN  unit  numbers  OFXLE  * 6 

On  the  initialization  pass,  the  title  “ACC  COMPENSATION 
INITIALIZATION"  and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  1 1.  See  Section  2.2  for  print  control  logic.  The  printed 
output  is  as  follows 
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<K5M»sra 


I 

I 

I' 

1 

tj 

I 

l 

I 

V 


Variable  Units  Description 

DVA  ft/s  compensated,  quantized  integral  of 

vehicle  specific  force  (incremental 
velocity)  along  x,  Y,  and  Z accelerome- 
ter input  axes  plus  errors  accumulated 
over  the  slow  cycle 


(7)  Subroutines  Called  (See  Section  2.3.15) 
MXV  * matrix  by  vector  product 


i 

i 

t 

i 

i 

j 

i 


2.3.10  GYRO  COMPENSATION  MODULE  (GCOMP) 

(1)  General  Description 

This  module  computes  the  compensation  for  gyro  bias,  scale- 
factor  error,  acceleration-sensitive  terms,  acceleration-squared 
sensitivities,  scale- factor  variation  with  rate,  output-axis  coupling, 
anisoinertia,  and  gyro  misalignments. 

(2)  Gyro  compensation  Module  Flow  Diagram 

Figure  1 depicts  the  general  logic  flow  of  the  gyro  compensation 

module. 

(3)  Input 


(a)  Module 

Initialization  File  (IFILE) 

FORTRAN 

unit  number: 

69 

FORTRAN 

format:  15, 

F20.10 

index 

Variable 

Default  Value 

Units 

Description 

1 

DT 

0.02 

s 

module  operating  frequency 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print 
otherwise  print) 

3 

OUTSW 

0.0 

— 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for 
printout 

5 

SPARE1 

0.0 

not  used 

6 

SPARE2 

0.0 

MlMM* 

not  used 
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Index 

Variable 

Default  Value 

Units 

7 

QMIS(l) 

1.0 

unity 

8 

QMIS (2) 

0.0 

unity 

9 

QMIS (3) 

0.0 

unity 

10 

QMIS (4) 

0.0 

unity 

11 

QMIS (5) 

1.0 

unity 

12 

QMIS (6) 

0.0 

unity  1 

13 

QMIS (7) 

0.0 

unity 

14 

QMIS (8) 

0.0 

unity 

15 

QMIS (9) 

1.0 

unity 

16 

— 

— 

17 

SFPO(l) 

-30.0 

ppm 

18 

SFP0(2) 

-30.0 

ppm 

19 

SFP0(3) 

-30.0 

ppm 

20 

BIAS (1) 

-0.34 

deg/h 

21 

BIAS (2) 

-0.34 

deg/h 

22 

BIAS{3) 

-0.34 

deg/h 

23 

SFMO(l) 

-44.0 

ppm  'j 

24 

SFH0(2) 

-44.0 

ppm  ' 

25 

SFMO ( 3 ) 

-44.0 

ppm  J 

Description 


compensating  transformation 
from  X,  Y,  Z gyro  input 
axis  to  X,  Y,  Z body  axes 


compensation 
positive  scale- 
factor  error 


compensation 
bias  error 
for 


computation 
negative  scale- 
factor  error 
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Index 

Variable 

Default  Value 

Units 

Description 

26 

QG3X (1) 

1.0 

unity \ 

27 

QGBX(2) 

0.0 

unity 

28 

QGBX(3) 

0.0 

unity 

29 

QGBX(4) 

0.0 

unity 

compensation  transformation 

30 

QGBX(5) 

0.0 

unity 

v from  X,  Y,  Z gyro  output 

’ (body)  axes  to  input,  output, 

31 

QGBX(6) 

1.0 

unity 

spin  axes  of  X gyro 

32 

QGBX(7) 

0.0 

unity 

33 

QGBX(8) 

-1.0 

unity 

34 

QGBX(9) 

0.0 

unity  ] 

35 

QGBY(l) 

0.0 

unity ^ 

i 

36 

QGBY (2) 

1.0 

unity 

37 

QGBY (3) 

0.0 

unity 

38 

QGBY (4) 

1.0 

unity 

compensation  transformation 

39 

QGBY (5) 

0.0 

unity ' 

from  X,  Y,  Z gyro  output 
f (body)  axes  to  input,  output, 

40 

QGBY (6) 

0.0 

unity  l 

spin  axes  of  Z gyro 

41 

QGBY (7) 

0.0 

unity  | 

42 

QGBY (8) 

0.0 

unity 

43 

QGBY (9) 

-1.0 

unity  i 

. 

44 

QGBZ(l) 

0.0 

unity  \ 

45 

QGBZ (2) 

0.0 

unity? 

^ compensation  transformation 
from  X,  Y,  S gyro  output 

46 

QGBZ(3) 

1.0 

unity f 

(body)  axes  to  input,  output, 
spin  axes  of  Z gyro 

47 

QGBZ (4) 

1.0 

unity  1 
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Description 


ndex 

Variable 

Default  Value 

Units 

48 

QGBZ (5) 

0.0 

unity 

49 

QGBZ (6) 

0.0 

unity  C( 

unity  ^ 

, S' 

unity 

50 

QGBZ (7) 

0.0 

51 

QGBZ (8) 

1.0 

52 

QGBZ (9) 

0.0 

unity 

53 

K0(1) 

-0.037 

deg/h/g  J c 

54 

K0(2) 

-0.037 

deg/h/g  \ ' 

55 

KQ(3) 

-0.037 

deg/h/g  J < 

56 

KI(1) 

1.16 

deg/h/g  | < 

57 

KI(2) 

1.16 

deg/h/g  / ■ 

58 

Kl(3) 

1.16 

deg/h/g  ) 

59 

KS  (1) 

-0,66 

deg/h/g  j 

60 

XSi2) 

-0.66 

deg/h/g  / 

61 

KS  (3) 

-0.66 

deg/h/g J 

62 

KII(l) 

0.0 

deg/h/g  J 

63 

KXItt) 

0.0 

deg/h/g2 > 

64 

Kll(3) 

0.0 

deg/h/g2l 

65 

u 

151000. 

gm-cm  /q 

2 

66 

XXX 

226. 

gm-cm 

67 

KSSU) 

0.0 

deg/h/g2 j 

63 

KSS(2> 

0.0 

deg/h/g2. 

69 

KSS(31 

0.0 

deg/h/g2] 

X I 

Y > gyro. 

zl 
X J 

y / gyco. 

Y 1 
X| 

^ Y > gyro. 

Z 1 

' y > gyro. 


UWH'f'Vt.’-—  •—  

spin-axis 

acceleration 

sensitive 

coefficient 


compensation  j x I 

input-axis  1 I 

acceleration-  \ ^ / ^ro* 

squared  sensitive!  1 

coefficient  I 

compensation  gyro  angular 
momentum 

compensation  moment  of  inertia 
about  output  axis 


compensation  Ml 

spin-axis  II 

acceleration-  < * ; gyro, 

squared  sensitive*  I 
coefficient  Ml 
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Index 

Variable 

Default  Value 

Units 

70 

KIO(l) 

0.0 

deg/h/g2 

1 

71 

KIO(2) 

0.0 

2' 

deg/h/g 

72 

KIO(3) 

0.0 

degA/g2 

73 

K0S(1) 

-0.15 

degA/g  j 

74 

KOS (2) 

-0.15 

deq/h/g2. 

75 

KOS  (3) 

-0.15 

deg/h/g2J 

76 

KIS(l) 

0.06 

degA/g2] 

77 

KXS  (2) 

0.06 

degA/g2! 

78 

KIS  (3) 

0.06 

degA/g  J 

79 

DELI 

14.0 

2 

gro-cm 

Description 

compensation  | Ji 

input  - output  1 

axis  acceleration-/ V 
squared  sensitive  1 
coefficient  [j 

compensation  f ; 

output  - spin  axxsl 
acceleration-  \ ' 

squared  sensitive  I 


squared  sensitivief 
coefficient  I 


z 


gyro. 


gyro. 


, gyro  spin-axis  minus  input- 
■ axis  moment  of  inertia 


80 

SFMl(l) 

0.0 

81 

SFM1(2) 

0.0 

82 

SFHl (3) 

0.0 

83 

SFPl(l) 

0.0 

84 

SFP1(2) 

0.0 

85 

SFPl (3) 

0.0 

86 

MODPDT 

6.0 

ppm/deg/s' 

compensation 
negative  scale- 

d 

ppm/rad/d 

( 

, factor  error  < 

varying  with  . 

r 

ppm/rad/s 

rate  | 

•J 

ppm/rad/s 

1 

1 compensation 
f positive  scale- 

n 

ppm/rad/s 

^ factor  error  < 

varying  with 

r 

ppm/rad/d 

rate 

t2) 

r 

tS 

module  print  interval 

gyro. 


gyro. 


(b>  Common  initialization  File  (PflLB) 
FORTRAN  unit  numbers  7 
FORTRAN  formats  IS,  F20.10 
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Index  Variable 


Default  Value 


Units 


Description 


1 WE 


0.729211514E-4 


rad/s  earth  rotation  rate 


2 RE 


20925640.0 


earth  radius 


3 G 


ft/s  nominal  gravity 


PRNTDT 


printing  frequency 


5 PBUP(l) 


not  used 


(4)  Call-Line  Data 


Input 


(T,  IENDF, 


>,  DVA, 


(a)  Call-Lino  Input 


DTHETZ) 

Output 


Variable  Units  Data  Type 


Description 


current  simulation  time 


IENDP  logical  INTEGER 


last-pass  indicator 


DXHETO  rad 


quantised  integral  of  angular  rate 
about  X,  V,  2 gyro  input  axes 
(fast-cycle  computation,  accumu- 
lated over  slow  cycle) 


<b)  Call-Line  Output 


compensated  quantised  incremental 
velocity  (fast-cycle  computations 
accumulated  over  slow  cycle) 


Variable  Units  Data  Typo 


Description 


DTHETZ 


compensated,  quantised  integral  of 
angular  rate  about  X,  Y,  Z gyro  input 
axes  (fapt-cycle  computations  accumu- 
lated over  slow  cycle) 
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(5)  Formulation 

(a)  Initialization  Function 

The  initialization  switch,  I NITS W,  is  initialized  to  zero  in  a 
DATA  statement.  The  following  functions  are  performed  on  the  first 
pass,  if  INITSW  » C,  IENDF  “ 0,  and  if  the  module  operating  cycle  time 
has  elapsed. 

• Read  and  print  initialization  file  (IFILE). 

• Read  common  initialization  file  (PFILE) . 

• Set  OFILE  « XFILE. 

• Convert  initialization  data  to  computational  units, 


SMI 

ta 

SFMl  * l.E  - 6 

SP1 

ta 

SFF1  * l.E  - 6 

SPO 

m 

SFPO  * l.E  - 6 

SMO 

m 

SFHQ  * l.E  - 6 

BIAS 

m 

BIAS  * 4.85E  - 6 

KI 

m 

K f /*  4.BSE  - 6/G 

1, 

KO 

m 

m * 4.85E  - 6/G 

KS 

m 

KS  • 4.8SE  - 6/G 

KII 

m 

KXI  * 4. BSE  - 6/G2 

KIS 

m 

KIS  * 4.85S  - 6/3  2 

KCS 

fa ' 

KOS  * 4. BSE  - 6/S2 

XSS 

m 

KSS  * 4.85E  - 6/32 

tub 

m 

KIO  * 4. BSE  - 6/32 

XXX 

m 

IKK  * 2.37E  - 6 

DELI 

m 

DELI  * 2.37E  - 6 

U 

• m 

H * 2.37E  - 6 

2-120 


where  the  conversion  factors  are 


lb- ft2 

2 

2.37E  - 6 gm-cm 

unity  = 

l.E  - 6 ppm 

deg/h/g 

4.85E  - 6 rad/s/g 

2 

deg/h/g  >= 

2 

4.85E  - 6 rad/s/g 

Define  the  matrix,  QOBG, 

axes  of  the  X,  Y,  and  Z gyros) 

from  the  second  row  (i.e.,  the  output 

it  it  i ft 

of  QGBX,  QGBY,  and  QGBZ,  respectively, 

QGBX(4) 

QGBX (5) 

QGBX (6) 

QOBG  => 

QGBY (4) 

QGBY (5) 

QGBY (6) 

QGBZ (4) 

QGBZ (5) 

QGBZ (6) 

1 

The  initialization  switch' is  set,  IN1TSW  » 1,  and 

the  current  simulation 

time  is  incremented 


TCG Y - T + DT 


Now  the  subroutine  returns  to  the  main  program. 


(b)  General  Function 

The  following  functions  are  porformod  every  module  operating 

cycle. 
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The  quantized  integral  of  the  angular  rate  about  the  X,  Y,  and 
Z gyro  input  axes  are  compensated  for  positive  scale-factor  error  when 
DTHETO  >_  0.0  by  the  factor 

T 1 = 1.  + SPO  + SP1  * DTHETO/DT 

and  for  negative  scale- factor  error  when  DTHETO  < 0.0  by  the  factor 
T 1 = 1.  + SMO  + SMI  * DTHETO/DT 

A bias  correction  is  added, 

DTHETO  = DTHETO  * T1  + BIAS 

i 

The  quantized  integral  of  angular  rate.  DTHETO,  is  compensated 
for  acceleration  effects  for  X,  Y,  and  Z gyro,  in  turn. 

First,  for  the  X gyro,  the  vector  DVA  is  transformed  from  the 
X,  Y,  Z accelerometer  input-axis  coordinates  to  accelerometer  (input, 
output,  and  pendulous)  coordinates 

DVG  =>  QGBX  * DVA 

Also,  for  the  X gyro,  the  vector  DTHETO  is  similarly  transformed  from 
the  X,  Y,  Z gyro  input-axis  coordinates  to  gyro  (input,  output,  spin) 
coordinates 

DTHETG  » QGBX  * DTHETO 
Similarly,  for  the  Y gyro 

DVG  » QGBY  * DVA 
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and 


DTHETG  = QGBY  * DTHETO 

and  for  the  Z gyro 


* 


DVG  = 

QGBZ  * 

DVA 

DTHETG  = 

QGBZ  * 

DTHETO 

From  the  transformed  vectors,  DVG  and  DTHETG,  the  acceleration 
effects  and  anisoinertia  compensation  are  computed  for  the  X,  Y,  and 
Z gyros 

DTHETO  o DTHETO  + KI*DVG(1)  + KO*DVG(2)  + KS*DVG(3) 

+(KQS*DVG{2) *DVG(3)  + KSS*DVG(3) 2 + KII*DVG(1)2 
+ KIS*DVG(1)*DVG(3)  + KI0*DVG{1)  * DVG(2))/DT 
- DELI*DTHETG(1)  * DTHETG ( 3) /(H*DT) 

Finally,  output  axis  coupling  compensation  is  accomplished  by  the 
following  computation  on  ail  passes  except  the  first  pass  after  initiali- 
zation, when  Kl  * 0,  First,  the  difference  between  DTHETO  and  DTH, 
where  DTH  - DTHETO  of  the  previous  pass  is  defined  as 

WDOT  « DTHETO  - DTH 

and  WDOT  is  resolved  about  the  X,  V,  Z gyro  output  axes,  as  DWO 

DWO  » QOBG  * WDOT 


and 


DTHETO  « DTHETO  + IXX  * DWO/(H  * DT) 

with  components  about  the  X,  Y,  and  Z gyro  input  axis. 

On  every  pass,  DTHETO  is  restored  as  DTH 

DTH  = DTHETO 

for  use  in  the  subsequent  pass.  K10  set  to  1 on  the  first  pass  after 
initialization. 

The  quantized  integral  of  angular  rate,  DTHETZ,  about  the  X,  Y, 
and  2 gyro  input  axes  is  compensated  for  misalignment 

DTHETZ  » QMIS  * DTHETO 

The  vector,  DTHETZ,  is  printad  and  the  simulation  time  incremented 

TCGY  * T + DT 

The  subroutine  now  returns  to  the  main  program. 

(6)  Output 

(a)  Print 

FORTRAN  unit  number)  OFILE  ■ 6 

On  the  initialization  pass,  the  title  "GYR  COMPENSATION  INITIALIZA- 
TION" and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  > 1.  See  Section  2.2  for  printe  control  logic.  The  printed 
output  is,  as  follows 


Variable  Units 


Description 


WIHI»ifSlaiWKw»iiV«i'ilji 


DTHETZ  rad  compensated  quantized  integral  of  angular 

rate  about  the  X,  Y,  Z gyro  input  axes  over 
the  slow  computation  cycle  plus  errors 


(7)  Subroutines  Called  (See  Section  2.3.15) 
MXV  = matrix  by  vector  product 
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2.3.11  LASER  GYRO  COMPENSATION  MODULE  (GCOMP) 


(1)  General  Description 

The  compensation  module  accepts  incremental  rotations  from  the 

simulated  laser  gyros  and  produces  compensated  incremental  rotations  in 

> 

body  coordinates.  The  module  contains  compensation  for  the  following 
error  sources: 


• gyro  input  axis  misalignment 

• scale  factor  error 

t scale  factor  turn-on  transient 

• gyro  bias 

• turn-on  transient  drift 


(2)  Gyro  Module  Flow  Diagram 

Figure  1 is  a flow  diagram  of  the  laser  gyro  compensation  module. 

The  module  is  initialized  on  the  first  pass  through  the  routine.  The 
compensation  module  initialization  file  (IFILE)  is  read  and  the  compensation 
parameters  are  converted  to  internal  program  units.  The  program  then 
performs  the  normal  module  operations.  An  analytical  description  of  the 
laser  compensation  equations  is  given  in  Volume  II » Section  5.2.3. 


(3)  Input 

(a)  Module  Initialization  File  (IFILE) 


FORTRAN  unit  number  » 69 

FORTRAN  format:  15,  F20.10 


/ 


Figure  1.  Laser  Gyro  compensation  Module  Flow  Diagram 
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Index 


Default 
Value  Units 


Description 


Variable 


1 

DT 

0.02 

s 

module  operating  frequency 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print, 
otherwise  print) 

3 

OUTSW 

0.0 

— 

not  used 

4 

XPILE 

6.0 

logical 

FORTRAN  unit  number  for  printout 

5 

SPAKE 1 

0.0 

— 

not  used 

6 

SPARE 2 

0.0 

— 

not  used 

7 

MODPDT 

6.0 

s 

module  print  interval 

8 

DB(1) 

0.7 

deg/hr  x 

9 

D6  (2) 

0.4 

deg/hr  ( 

gyro  bias 

10 

DB(3) 

-0.6 

deg/hr  ) 

11 

DTA(l) 

0.06 

deg/hr  \ 

12 

DTA<2) 

-0.03 

deg/hr  > 

transient  drift  magnitude 

13 

DTA(3) 

-0.05 

deg/hr  ' 

• 

14 

DTC(l) 

45.0 

min 

15 

DTC(2) 

45.0 

min 

transient  drift  time  constant 

16 

DTC(3) 

45,0 

min 

17 

SFTAU) 

0.8 

ppm  | 

SFTA<2) 

-0.7 

ppm  | 

transient  scale  factor  amplitude 

19 

sm<3) 

1.0 

ppm  t 

20 

sftcu) 

45,0 

min  | 

21 

SFTC(2) 

45.0 

min  | 

transient  scale  factor  time  constant 

22 

SflC(3) 

45.0 

min  1 

23 

KW{1) 

1.0 

ppm  ^ 

' 

24 

KW<2) 

20.0 

ppm 

1 

25 

RW{3) 

50.0 

ppm  j 

1 

26 

KW(4) 

40.0 

ppm 

f gyro  $F  and  gyro  input  axis  (IA) 

27 

KW£S) 

1,5 

\ 

ppm 

{ misalignment  matrix 

28 

KW(6) 

30.0 

PP» 

f » main  diagonal  *»  SP  bias  (nan) 

29 

KW{?) 

-30.0 

ppm 

Is  off  diagonal  ■ IA  mis  (microrad 

30 

KW(8) 

-40.0 

SHE*9 

I 

31 

KM(9)  V 

-1.0 

ppm  t 

r 
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(b)  Common  Initialization  File  (PFILE) 
FORTRAN  unit  number:  7 
FORTRAN  format:  15,  F20.10 


tf 

1 

t. 

I 

Default 

Index 

Variable 

Value 

Units 

Description 

>. 

V 

/• 

1 

WE 

0.729211514E-4 

rad/s 

earth  rotation  rate 

* 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

ft/s2 

nominal  gravity 

i 

i 

4 

PFNTDT 

6.0 

s 

printing  frequency 

9 

I 

5 

PBUF(l) 

0.0 

— 

not  used 

(4)  Call-Line  Data 


Input 

(t7  IENDF,  DTHETO,  DVA,' 


DTHETZ) 

Output 


(a)  Call-Line  Input 


Variable 


Units 


Data  Type 


Description 


T 

IENDF 

DTHETO 


s 

logical 

rad 


REAL 

INTEGER 

REAL 
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i..V . 


Current  simulation  time 

last-pass  indicator 

quantized  integral  of  angular  rate 
about  X#  V,  Z gyro  input  axes 
{fast-cycle  computation#  accumu- 
lated over  slow  cycle) 

not  vised 


REAL 


(b)  Call-Line  Output 


Variable  Units  Data  Type 


Description 


DTHET2  rad  REAL  compensated,  quantized  integral  of 

angular  rate  about  X,  Y,  2 gyro 
input  axes  (fast-cycle  computations 
accumulated  over  slow  cycle) 


(5)  Formulation 

( a ) Initialization  Function 

The  initialization  switch,  INITSW,  is  initialized  to  zero  in  a DATA 
statement.  The  following  functions  are  performed  on  the  first  pass,  if 
INITSW  = 0,  IENDF  = 0,  and  if  the  module  operating  cycle  time  has  elapsed, 

• Read  and  print  initialization  file  (IFILS). 

• Read  common  initialization  file  (PFILE) . 

• Set  OFILE  =>  XFILE, 

• convert  the  initialization  data  to  internal  program  units. 

Internal  units  ares  radians,  feet,  and  seconds. 


DB 

- Ill 

DB  * DTR/3600. 

DTA 

tx 

DTA  * DTR/ 3600 

DTC 

6* 

DTC  * 60. 

srn 

X* 

SFTA  * l.E-6. 

SFfC 

t* 

SFfC  * 60. 

KW 

631 

KW  * l.E-6 

where  DTR  equals  degrees-to- radians  conversion  factor. 

• Compute  the  exponential  drift  and  exponential  scale  factor  coefficients. 

EXPO  - EXP (-DT/DTC  ) 

EXPSF  - EXP (-DT/SFTC  ) 
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The  vector  DTC  contains  the  time  constants  of  the  exponential  drifts 
and  SFTC  contains  the  time  constants  of  the  exponential  scale  factors. 
The  elements  of  EXPD  and  EXPSF  are  set  to  aero  if  the  corresponding 
time  constants  are  nonpositive.  The  difference  equation  for  each  of 
the  terms  is  of  the  form 

DT(n+l)  « DT(n)  * EXPD 
St(N+1)  - ST<n)  * EXPSF 

Thus,  the  individual  transients  are  set  to  zero  if  the  associated  time 
constant  is  not  positive.  This  allows  the  user  to  delete  compensation 
for  any  of  the  transient  terms  by  setting  the  amplitude  and  time 
constant  to  zero. 

The  initialization  switch  is  set,  INITSW  » 1,  and  the  current  simulation 
time  is  incremented 

TCGY  » T + DT 

Now  the  subroutine  returns  to  the  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating  cycle. 

• Compute  the  transient  drift  and  scale  factor  compensation. 

* 

DTA  - DTK  * EXPD 

SFT k - SFTA  * EXPSF 

The  transient  terms  are  set  to  zero  if  the  magnitude  becomes  less  than 
l.E-10  to  prevent  underflow. 
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• Add  the  scale  factor  transient  to  the  sea] a factor  bias.  Insert 

* 

the  sum  on  the  main  diagonal  of  the  rate  sensitive  matrix  KW. 


KW(K)  = SF(I)  + SFTA(I) , where  K * diagonal  elements 


« 


The  off  diagonal  elements  of  KW  contain  the  IA  misalignments. 


• compute  the  compensated  incremental  rotation  DTHETZ. 


DTHETZ 


DTHETO  - { DB  + DTA  ) DT] 


where 

DTHETO 

DB 

DTA 

KW 

* 

I 

The  vector, 


= quantized  angles  from  the  simulated  laser  gyros 
« gyro  bias  compensation 
= transient  drift  compensation 
« angular  rate  sensitive  coefficient  compensation 
= identity  matrix 

DTHETZ,  is  printed  and  the  simulation  time  incremented 


TCGY  “ T + DT 


The  subroutine  now  returns  to  the  main  program. 

(6)  Output 

(a)  Print 

FORTRAN  unit  number:  OFILE  * 6 

On  the  initialization  pass,  the  title  “LASER  GYRO  COMPENSATION 
INITIALIZATION**  and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  HODPDT  intervals  when  PRNTSW  >,  1. 
See  Section  2.2  for  prints  control  logic.  The  printed  output  is,  as 
follows 


Variable 

Units 

Description 

DTHETZ 

rad 

Compensated  quantized  integral  of  angular 
rate  about  the  X»  Y,  2 gyro  input  axes  over 
the  slow  computation  cycle  plus  errors 
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Subroutines  called  (See  Section  2.3.15) 
MXV  = matrix  by  vector  product 
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2.3.12  ATTITUDE  AND  VELOCITY  ALGORITHM  MODULE  (ALG) 

(1)  General  Description 

The  first  segment  of  this  module,  the  “attitude''  algorithm  pro- 
cesses the  incremental  angle  data,  A6,  (from  the  gyro  compensation 
module)  to  update  the  body-to-inertial  transformation,  c£,  the  initial 
value  of  which  is  computed  in  the  initilization  pass.  Either  a qua- 
ternion or  a direction  cosine  matrix  update,  of  first  through  third 
order,  may  be  employed,  as  specified  by  the  user. 

The  second  segment  uses  C*  to  transform  the  incremental  velocities, 
Av,  (from  the  accelerometer  compensation  module)  to  the  inertial  frame, 
where  they  may  be  sunned,  and  passed  on  to  the  navigation  module. 

An  additional  transformation  from  the  inertial  frame  to  the  navi- 

o * Jl 

gator's  earth-fixed  frame,  C\  , is  applied  to  before  it  is  passed 
on  to  the  latter  module  for  use  in  attitude  computation. 

(2)  Attitude  and  Velocity  Algorithm  Module  Flow  Diagram 

The  general  logic  flow  of  the  ALG  module  is  shown  in  Figure  1. 

(3)  Input 

(a)  Module  Initialization  File  (IPILE) 

FORTRAN  unit  number:  70 
FORTRAN  format:  IS,  F20.10 
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Default 


Index 

Variable 

Value 

Units 

Description 

1 

DT 

0.02 

secs 

module  operating  frequency 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print,  other- 
wise print) 

3 

OUTSW 

0.0 

— 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for  printout 

5 

QORDCM 

1.0 

logical 

algorithm  switch jo  - 

quaternion 
direction 
cosine  matrix 

6 

MODPDT 

6.0 

secs 

module  print  interval 

7-10 

— 

— 

— 

not  used 

11 

ORDER 

3.0 

logical 

order  specification 
for  quaternion  or 
direction  cosine 
(dcm)  update 

first-order- 1 
second-order - 
third-order®  3 

12 

DTNRM 

4.0 

secs 

time  increment  between  quaternion 
or  dcm  normalization  calculation 

13 

DTSLOW 

0.0 

™ 

not  used 

14 

LATERR 

0.0 

deg 

initial  vehicle  latitude  error 

15 

LONERR 

0.0 

deg 

initial  vehicle  longitude  error 

16 

WANERR 

0.0 

deg 

initial  vehicle  wander  angle  error 

17 

PITERR 

0.0 

deg 

initial  vehicle  pitch  error 

18 

ROLERR 

0.0 

deg 

initial  vehicle  roll  error 

19 

VAWERR 

0.0 

deg 

initial  vehicle  yaw  error 

(b)  Common 

Initialization  File 

(PFILE) 

FORTRAN  unit  number t 7 
FORTRAN  format:  15,  F20.10 

Default 


Index 

Variable 

Value 

Units 

Description 

1 

WE 

0.7292U5147E-4 

rad/sec 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

U 

32.2 

ft/sec2 

nominal  gravity 

4 

PRNTDT 

6.0 

Secs 

printing  frequency 

5 

ILAT 

0.0 

dt>'j 

initial  latitude 

6 

ILCN 

0.0 

deg 

initial  longitude 
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u 

£ 

f- 

Index 

Variable 

Default 

Value 

Units 

Description 

7 

WANDER 

0.0 

deg 

initial  wander  angle 

?; 

>..• 

8 

•*•••* 

0.0 

— 

not  used 

f- 

'? 

c 

9 

ROLL 

0.0 

deg 

initial  roll  angle 

l. 

if 

a 

10 

PITCH 

0.0 

deg 

initial  pitch  angle 

t: 

tv- 

11 

YAW 

0.0 

deg 

initial  yaw  angle 

(4)  Call  Line  Data 


INPUT 

t,  iendpT^dtheti^  DV, 


(a)  Call  Line  Input 


Variable 

T 

IENDF 

DTHETI 


Units 

secs 


Data  Type 


logical  INTEGER 


ft/soc  REAL 


DVN,  DCM 
OUTPUT 


Description 

current  simulation  time 

last  pass  indicator 

coiqpensated  quantized  inte- 
gral of  angular  rate  about 
X,Y,z  gyro  input  axes  (fast 
cycle  computations  accumu- 
lated over  slow  cycle) 

compensated  quantized  inte- 
gral of  vehicle  specific 
force  along  X,Y,z  acceler- 
ometer input  axes  (fast 
cycle  computations  accumu- 
lated over  slow  ayele) 


(b)  Call  Line  Output 


Variable 

DVN 


Units  Data  Type 
ft/sec  REAL 


DCM  units  REAL 


Description 

quantized  integral  of 
vehicle  specific  force  in 
the  inertial  frame  of  the 
local  level  navigation 
module  (LLN) 

direction  cosine  matrix 
(body  to  earth-fixed  frame 
transformation) 


(5)  Formulation 


(a)  Initialization  Function 

The  initialization  switch/  INITSW,  is  initialized  to  zero  in  a 
DATA  statement.  The  following  functions  are  performed  on  the  first 
pass,  if  INITSW  = 0,  IENDF  = 0,  and  if  the  module  operating  cycle  time 
has  elapsed. 


• Read  and  print  initialization  file  (IFILE) . 

• Read  common  initialization  file  (PFXLE) . 


• Set  OFILE  » XFILE 
NQDCM  » QORDCM 
NORDER  ■ ORDER 


• Initialize 


DVN_  =>o.O 
DTHPRE  « 0.0 

• Define  the  following  quantities, 
WORM  - T + DTORM 


and  convert  the  angles  in  degrees  to  radians, 


C0SP  - COS 

cosy  - cos 

COSR  “ COS 
COSLAT  * COS 
C0SL0N  » COS 
C0SW  - COS 


{ (PXTOH+PXTERR) /RDTODG) 

( (YAW+YAWBRR) /RDTODG) 

( (ROLL+ROLLERR) /RDTODG) 

( (ILAT+LATERR) /RDTODG) 

( ( XLON+LONERR) /RDTODG) 

( (WANDER+WAHERR) /RDTODG) 
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SINP  - SIN  ((PITCH+PITERR) /RDTODG) 

SINY  = SIN  ( (YAW+YAWERR) /RDTODG) 

SINR  = SIN  ( (ROLL+ROLLERR) /RDTODG) 

SINLAT  = SIN  ( ( ILAT+LATERR) /RDTODG) 

SINLON  = SIN  ((ILON+LONERR) /RDTODG) 

SINW  = SIN  ( (WANDER+WANERR) /RDTODG) 

where  the  conversion  factor, 

RDTODG  = 57.29577951  deg/rad 

Further  the  tetri*.  2|B,  a ***  t0  oomputational  frame  transfer- 
nation  matrix  is  defined, 

. C°SP*SIm  -SIHE*SIHF.SI»F-COSF.COSH|l-COSR.SniP.SOT+COSY.SIN8 
QB=  COSP.COSY  -SINR*SINP*COSY+COSR*SIHYl-COSR*SIMP*COSY-SINR*SINY 

SI"P  COSP.SINR  | COSP.COSR 

from  the  tingle  axis  rotations  (Appendix  a of  Volunm  II) 

8PB  - 2 (-TT/2)  *(-„)  Z(-Y)  Y(-P)  X(— R)  X(-x, 

‘ ”~OMl  40  lne“ial  W -~ion 

J 


9 SINW*C0S1AT  I COSW*COSLAT  I 8If)IJW 

J ‘*COSH*SXN|^N«SXt)W*SXNIAT*COSLOtl|  SINW.SINUJN-SINWV?^ 

. J"*  **  *°  frMe  transformation  matrix,  (JIB,  is  defined 

by  the  matrix  Multiplication, 

* * * 

QIB  « QIP  * qpb 

The  vector,  QUAT,  is  initialised  as, 

CUATU)  » (U+giB(l)+^iB(S)  + QIBO))/^1^ 

QUAT (2)  * (QIB(0)-qib{6; )/(4*QUAT(l) ) 

QUAT (3)  « (QIB (3) -QIB (7) )/(4*QUAT(l) ) 

QUAT (4)  - (QIB(4)-QIB(2))/(4*QUAT(1)) 


for  a quaternion  update  or  when  NQPCM  = 1,  for  a direction  cosine 
matrix  update/  QUAT  is  initialized  as 

QUAT  ® (0.0/  0.0/  0.0,  0.0) 

The  vector,  DTHP,  the  previous  pass  value  of  DTHETI  is 
DTHP  ■ (0.0,  0.0,  0.0) 

The  initialization  switch  is  reset 

IN1TSW  o 1 

and  the  simulation  time  incremented 

TALG  =>  T + DT 

The  subroutine  now  returns  to  the  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating 

aycle. 

* 

Define  DCMOLD,  the  direction  cosine  matrix  of  the  previous 


pass*  as 

* * 

DCMOLD  - QXB 

* 

* 

* * 

for  use  in  the  interpolated  values  of  DCN  and  DCKMIO 
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The  vectors  DTHETA  and  DTHP  are  defined 


tt 


DTHETA  = DTHETI 
DTHP  = BTHETI 


A direction  cosine  matrix#  DCM,  is  computed  by  one  of  two 
| update  methods:  when  NQDCM  = 0,  a quaternion  update  calculation  or 

j NQDCM  = 1#  a direction  cosine  matrix  update  calculation.  In  either 

\ case  there  is  a choice  of  a first  order#  second  order,  or  third  order 

j.  update. 

V 

!• 

v 

| The  quaternion  calculation  proceeds  with  the  definition  of  the 

\ ★ 

' matrix#  UDELT 


! 

-DTHETA (1) 

l 

-DTHETA (2) 
j DTHETA (3) 

, -DTHETA (3) 

UDELT  = 

DTHETA (1) 1 
DTHETA (2) 

DTHETA (3) | 

( 

-DTHETA (3) 

I -DTHETA (2) 

1 DTHETA (1) 

DTHETA (2) 

1 -DTHETA (1) 

1 

1 

1 

where  the  diagonal  elements  are  undefined.  (See  Vol.  XX,  Section  7 
for  quaternion  update  discussion.) 
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For  the  first  order  quaternion  update,  when  NORDER  = 1,  the  terms  of 
the  quaternion,  QUAT,  are 

QUAT(l)  = QUAT (1)  + ( -DTHETA ( 1 ) *QUAT { 2 ) 

-DTHETA ( 2 ) *QUAT ( 3 ) 

-DTHETA (3) *QUAT (4 ) ) /2 


QUAT (2)  = QUAT (2)  + ( DTHETA (1 ) *QUAT(1) 

+DTHETA ( 3 ) *QUAT ( 3 ) 
-DTHETA (2) *QUAT(4)/2 


QUAT (3)  = QUAT (3)  4-  ( DTHETA (2) *QUAT(1) 

-DTHETA ( 3 ) *QUAT ( 2 ) 
+D0HETAU)  *QUAT{4)  )/2 


QUAT (4)  a QUAT (4)  + ( DTHETA (3) *QUAT(I) 

+DTHETA (2) *QUAT (2) 

-DTHETA ( 1 ) #QUAT ( 3 )) /2 

where  the  terms  on  the  right  refer  to  the  values  of  QUAT  on  the  previ- 
ous pass. 


The  dot  product,  UDTH2,  is  defined  for  use  in  both  the  second  and 
third  order  quaternion  update  as 

UDTHS  « -(DlHETAUi2  + DTHETA  (2) 2 4-  DTHETA  (3) 2) 

The  second  order  quaternion  update  is  calculated,  when  NORDER  “2. 
Tim  terms  of  the  quaternion  are  expanded  for  clarity. 

QUAT(l)  « QUAT(l)*  (1+UDTH2/8)  4*  {-DTHETA (1) *QUAT (2) 

-DTHETA (2) *QUAT<3) 

-DTHETA (3)*QUAT(4))/2 

QUAT {2)  « QUAT (2  } # { 1+UDTH2/8 ) 4*  ( DTHETA(l) *QUAT(1) 

4*DTHETA  (3)  *QUAT  ( 3 J 
-DTHETi\(2)  *QUAT(4)  )/2 

QUAT {3)  - QUAT(3)*(l+UDTH2/8)  4-  ( DTHETA(2)  *QUAT(1) 

-DTHBTA(3)*QUAT(2) 

+DTHETA(1)  *QUAT(4)  )/2 
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j 


QUAT (4)  = QUAT (4) * (1+UDTH2/8)  + ( DTHETA (3 ) *QUAT(1) 

+DTHETA (2 ) *QUAT (2 ) 
-DTHETA { 1 ) *QUAT (3) )/2 


| I 

4:  %' 


The  third  order  quaternion  update  is  calculated,  when  NORDER  = 3. 
The  terms  of  the  quaternion  are  expanded  for  clarity.  The  cross  prod- 
uct, LDTH,  is  defiled  for  the  angles,  DTHETA,  and  DTHPRE  of  the  previ- 
ous pass  DTHETA. 

LDTH  = DTHPRE  X DTHETA 

* 

These  cross  products  are  redefined  in  a matrix,  LDELT2. 


LDELT2 


1 

-LDTH(l) 

1 -LDTH (2) 

| LDTH (3) 

-LDTH (3) 

LDTH (1)  j 

— 

-LDTH (2) 

LDTH{2)  • 

1 

-LDTH (3) 

1 

j -LDTH(l) 

LDTH(l) 

LDTH (2)  j 

LDTH (2) 

where  the  diagonal  elements  are  undefined 
QUAT(l)  =»  QUAT(l)  * (1+UDTH2/8) 


QUAT{2) 


QUAT (3) 


=»  QUAT(l)  * (1+UDTH2/8) 

4-QUAT ( 2 ) * (-  (1/2+UDTH2/48)  *DTHETA(1)  -LDTH(l)/24) 
+QUAT(3)  * (-  (1/2+UDTH2/48)  *DTHETA(2)~LDTH(2)/24) 
+QUAT{4)  * (-(1/2+UDTH2/48) *DTIIETA(3)-LDTH(3)/24) 

*=  QUAT  (2)  * (1+UDTH2/8) 

+QUATU)  * ( (1/2+UDTH2/ 48)  *DTHETA  (1)  + LDTH  ( 1 ) /24 ) 
+QUAT(3)  * ( (1/2+UDTK2/48) *DTHETA (3)+LOTH{3)/24) 
+QUAT(4)  * ( - { 1/2+UDTH2/48 ) *DTHETA ( 2 ) -LDTH ( 2 ) /24 

a QUAT (3)  * U+UDTH2/8) 

+QUAT(1)  * ( a/2+UDTH2/48)*DTHETA(2)+LDTH(2)/24) 
+QUAT<2)  * ( - ( 1/2+UDTH2/ 48 ) *DTHETA ( 3 ) -LDTH ( 3 ) /24 ) 
+QUAT(4)  * { U/2-4-UDTH2/48)*DTliE'I,Aa)+LDTHa)/24) 


QUAT (4)  “ QUAT (4)  * (1+UDWI2/8) 

+QUATU)  * { (1/2+UDTH2./48) #DTHETA (3)+LDTH(3)/24) 
+QUAT(2)  * ( a/2+UDTH2/48)*DTHETA(2)+LDTH(2)/24) 
+QUAT(3)  * (-(1/2+UOTH2/48)  *DTHETA(l)-LDTH(l)/24) 
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If  NORDER  i-  1,  2,  or  3,  the  statement,  "ORDER  NOT  PROPERLY  SPECIFIED" 
is  printed,  the  last  pass  switch  is  set,  IENDF  = 1,  and  the  subroutine 
returns  to  the  main  program. 


The  simulation  time  is  tested  and  if  the  interval  DTNRM  has 
elapsed  or  T >_  TNORM 

the  quaternion  is  normalized 

QUAT  = QUAT/[QUATU)2  + QUAT(2)2  + QUAT(3)2] 


and  the  normalization  time  is  incremented 

TNORM  = T + DTNRM 

* 

The  quaternion  is  converted  to  the  direction  cosine  matrix,  DCM. 


First  define: 

Q1S 

S 

QUAT ( 2 ) 2 

Q2S 

cs 

QUAT (3) 2 

Q3S 

ss 

QUAT (4) 2 

QQ1 

C3 

QUAT(l)  * 

QUAT ( 2 ) 

Q02 

C8 

QUAT ( 1 ) * 

QUAT {3) 

QQ3 

C3 

QUAT(l)  * 

QUAT (4) 

Q12 

63 

QUAT  (2)  * 

QUAT (3) 

Q23 

C3 

QUAT (3)  * 

QUAT (4) 

Q31 

a 

QUAT (4)  * 

QUAT(2) 

Then 


DCM  *> 


l-2*(Q2SfQ3S)  j 2*{Q12-Q03)  ] 2*(Q31+Q02) 

2MQ12+Q03)  [ 1-2MQ3S-Q1S)  I 2*(Q23+Q01) 
2*(Q31-Q02)  j 2MQ23+C01)  | 1-2*(Q1SK>2S) 


An  alternative  to  the  quaternion  update  calculation  is  the  direc- 
tion cosine  matrix  update. 

* 

For  the  first  order  update,  when  NORDER  «*  1,  the  matrix,  DIM  is 
defined. 
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1 

★ 

DTM  * DTHETA(3) 
-DTHETA ( 2 ) 


DTHETA(2) 
-DTHETA (1) 
1 


For  the  second  order  direction  cosine  matrix  update,  vrtien 

NORDER  = 2,  the  following  terms  are  calculated  and  added  to  the  first 
* 

order  matrix,  DTM. 

DTM  = 

-( DTHETA <2)2+DTHETA( 3 )2I  DTHETA (1) *DTHETA(2)  I DTHETA (3) *DTHETA(1) 
+1/2*  DTHETA (1)*DTHETA( 2)  |~<  DTHETA  (3)2+DTHETA(l) 2 ) J DTHETA (2 ) *DTHETA (3) 

DTHETA { 3 ) * DTHETA ( 1 ) j DTHETA (2)+DTHETA{ 3)  (^(DTHETA(l) 2+DTHETA(2) 2) 


First  Order 


DTM(l)  I DTM  (2)  | DTM(3) 

DTM (4)  1 DTM(S)  ! DTM (6) 
I ' 

DTM (7)  | DTM (8)  j DTM(9) 


For  the  third  order  direction  cosine  matrix  update,  when  NORDER  •» 
3,  the  following  terms  are  caculated  and  added  to  the  second  order 
matrix,  DTM, 

First  define  the  term,  D 

D **  0.16666866666666  * (DTWETA(1)2+DTHETA(2)2+DTHETA(3)2) 

Then  define  the  third  order  terms  Cl,  C2,  and  C3.  Xn  this  version 
these  terms  are  set  to  zero, 

Cl  a o.O 
C2  ■ 0.0 
C3  » 0.0 

due  to  ttu>  infinite  accelerations  generated  in  the  available  version  of 
PRQFGEN.  The  correct  terms  are  defined  in  comment  statements  for  the 
third  order 
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Cl  = 0.8333333333333*D-1* (DTHETA(3) *DTHPRE(2)-DTHETA(2) *DTHPRE(3) ) 
C2  = 0.8333333333333*0-1* (-D1HETA(3) *DTHPRE(1)+DTHETA(1) *DTHPRE(3) ) 
C3  = 0.8333333333333*D-1*(DTHETA(2) *DTHPRE(1)-DTHETA{1) *DTHPRE(2) ) 


Thus, 


Second  Order 


DTM  = 


DTM(l)  J DTM (2)  , 01*1(3) 

DTM (4 ) I DTM(5)  ! DTM (6) 
l . 

DIM  (7)  | DTM (8)  I DTM (9) 


+ 


I 

0 I 

-D*DTHETA(3)+C3  ] 
D*DTHETA (2 ) ~C2  j 


D*DTHETA(3)-C3 

0 

-D*DTHETA { 1 ) +C1 


I 

| -D*DTHETA (2 ) +C2 
| D*DTHETA (1 ) -Cl 

I 0 


If  NORDER  </  1,  2,  or  3,  the  statement,  "ORDER  NOT  PROPERLY  SPECIFIED" 
is  printed,  the  last  pass  switch  is  set,  IENDF  <=>  1,  and  tho  subroutine 
returns  to  the  main  program. 

The  matrix,  DTM,  is  multiplied  by  the  body  to  inertial  frame  trans- 

* 

formation  matrix,  Q1B,  to  obtain  the  direction  cosine  matrix  of  the 
first,  second,  or  third  order  update  as  a body  to  inertial  frame  trans- 
formation. 

DCH  ■ QIO  * DTK 

* 

The  direction  cosine  matrix,  DCM,  is  normalized  at  intervals  of 
DTORM  seconds  or  each  time,  T > l’NORM.  Normalization  takes  the  form 
of  the  first  order  orthonorraalixation, 

* * 1 * * T * • 

DCM  * DCM  - ~ * DCM  * (DCM  * DCM  - I) 

« 

where  X is  the  identity  matrix. 

Hie  normalization  time  is  incremented 
WORM  - T ♦ DTNRM 
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The  normalized  direction  cosine  matrix  is  stored  in  the  body  to 

* 

inertial  irame  transformation  matrix,  Q IB,  for  use  on  the  next  pass. 

* * 

QIB  = DCM 

* 

Now  form  approximate  midpoint  direction  cosine  matrix,  DCMMID, 
from  DCM  of  this  and  previous  pass. 

DCMMID  = (DCM  + DCM0LDJ/2 

* 

Using  the  approximate  midpoint  direction  cosine  matrix,  DCMMID, 
obtained  either  from  the  quaternion  or  directio  cosine  matrix  update, 
the  compensated  quantized  integrals  of  the  specific  force,  DV,  are 
transformed  to  the  inertial  frame 

DVI  ■ DCMMID  * DV 


The  quantized  specific  force  increments,  DVX,  are  summed  to  be 
compatible  with  the  LLN  module  operating  frequency.  (DVN  is  zeroed 
in  the  LLN  module)  and  the  reference  frame  is  transformed  from  the 
ALg  inertial  frame  to  the  LLN  inertial  frame. 

DVN  (1)  * DVN(l)  + DVX (3) 

DVH ( 2)  « DVN (2)  + DVX (1) 

DVN  (3)  «*  DVNO)  + DVX  (2) 


The  direction  cosine  matrix  is  converted  from  a body  to  inertial 
frame  to  a body  to  earth-fixed  frame  transformation. 


* 

DCM  - 


1 

SWET 

1 

0,0 

l cwar 

CWET 

1 

1 

i 

0.0 

[ -SWET 

0.0 

1.0 

f °’° 

1 

**  v ■ 

* 

• DCM 
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where 


CWET  = COS (WE  * T) 

SWET  = SIN (WE  * T) 

Finally  if  the  update  order  is  third  order,  NQRDER  ® 3,  the 
quantized  integral  of  angular  rate,  DTHETA  is  stored  in  DTHPRE  for 
use  in  the  next  pass 

DTHPRE  = DTHETA 


The  quantized  integral  of  specific  force,  DVN,  the  direction 
* — — 

cosine  matrix,  DCM,  and  the  quaternion,  QUAT  (if  NQDCM  “ 0)  are  printed 
arid  the  simulation  time  is  incremented 

TALG  “ T + DT 

(6)  Output 

(a)  Print 

FORTRAN  unit  number*  QFXLE  -6 

On  the  initialization  pass  tire  title  "ALGORITHK  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  NODPDT  inotervals  when 
PRNTSW  w 1,  See  Section  2.2  of  Vol.  Ill  for  print  control  logic. 
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The  printout  is  as  follows s 


Variable 

DVN 


yUAT 

DCM 


Units 

ft/s 

unity 

unity 


Description 

quantized  integral  of  specific  force  along 
the  X,Y,Z  accelerometer  input  axes  in  the 
inertial  frame  of  the  local  level  naviga- 
tion module  (LLN) 

quaternion 

the  direction  cosine  matrix  (to  transform 
from  the  body  frame  to  the  earth  fixed 
frame) 


(7) 


Subroutines  Called  (See  Volo  III,  Section  2,3.15) 


MTXM  - Matrix  transpose  by  matrix  product 
MXM  - Matrix  by  matrix  product 
MXV  - Matrix  by  vector  product 


2.3.13  LOCAL  LEVEL  NAVIGATOR  MODULE  (LLN) 

(1)  General  Description 

The  software  algorithms  for  a local-level  wander-azimuth  navigator 
are  incorporated  by  this  module.  The  inertially  referenced  incremental 
vehicle  velocity,  passed  over  from  the  attitude  and  velocity  algorithm 
module  (ALG) , is  transformed  to  local-level  wander-azimuth  coordinates, 
and  then  used  to  update  the  vehicle  current  position  and  velocity.  The 
body-to-"new"-earth-fixed  transformation  from  the  ALG  module,  is  further 
transformed  to  the  "new”  computational  frame,  and  used  in  attitude  com- 
putation. An  altimeter  loop  is  also  employed,  using  the  simulated  bar- 
oaltimeter  measurement  from  the  altimeter  module  (ALTI),  for  the  purpose 
of  stabilizing  the  vertical  channel. 

(2)  Local-Level  Wander- Azimuth  Navigator  Module  Computational 
Flow  Diagram 


The  general  flow  logic  of 

the  LLN  module 

is  illustrated  in  Figure  1. 

(3) 

Input 

(a)  Module  : 

Initialization  File  (IFILE) 

FORTRAN 

unit  number: 

80 

FORTRAN 

format:  15, 

F20.10 

Index  Variable 

Default 

Value 

Units 

Description 

1 

DT 

0.02 

s 

module  operating  cycle  time 

2 

PRNTSW 

1.0 

logical 

print  switch  (0  - no  print, 
otherwise  print) 

3 

OUTSW 

0.0 

- 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for 
printout 

5 

SPARE  1 

0„0 

- 

not  used 

6 

MODPOT 

6.0 

s 

module  print  cycle  time 
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Figure  1.  Local  Level  Navigator  Module  (Sheet  1 of  2) 
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Figure  1 


Local  Level  Navigator  Module  (Sheet 
2-152 


of  2) 


Index 

Variable 

Default 

Value 

Units 

Description 

7 

ALTERR 

0.0 

ft 

initial  altitude  error 

8 

VERR(l) 

0.0 

ft/s 

initial  velocity  error 
(east) 

9 

VERR  ( 2 ) 

0.0 

ft/s 

initial  velocity  error 
(north) 

10 

VERR ( 3) 

0.0 

ft/s 

initial  velocity  error 
(up) 

11 

LATERR 

0.0 

deg 

initial  latitude  error 

12 

LONERR 

0,0 

deg 

initial  longitude  error 

13 

CVD1 

0.06 

1/s 

first  vertical  damping 
coefficient 

14 

CVD2 

0.00162 

1/s2 

second  vertical  damping 
coefficient 

15 

CVD3 

0.0000162 

1/s3 

third  vertical  damping 
coefficient 

(b) 

Common  Initialization  File  (PFILE) 

FORTRAN 

unit  number:  7 

FORTRAN 

format:  15,  F20. 

10 

Index 

Variable 

Default 

Value 

Units 

Description 

1 

WE 

0. 729211514E-4 

rad/s 

earth  rotation  rate 

2 

RE 

20925640.0 

ft 

earth  radius 

3 

G 

32.2 

2 

ft/s 

nominal  gravity 

4 

PRNTDT 

6.0 

s 

printing  frequency 

5 

ILAT 

0.0 

rad 

initial  latitude 

6 

I LON 

0.0 

rad 

initial  longitude 

7 

WONDER 

0.0 

rad 

initial  wander  angle 

8 

IH 

0.0 

ft 

initial  altitude  above 
sea  level 
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Index 

Variables 

Default 

Value 

9 

ROLL 

0.0 

10 

PITCH 

0.0 

11 

YAW 

0.0 

12 

DROLL 

0.0 

13 

DPITCH 

0.0 

14 

DYAW 

0.0 

15 

IV(1) 

0.0 

16 

IV  (2) 

0.0 

17 

IV  (3) 

0.0 

10 

IAX 

0.0 

19 

IAY 

0.0 

20 

IAZ 

0.0 

Units 

Description 

rad 

initial  roll  angle 

rad 

initial  pitch  angle 

rad 

initial  yaw  angle 

rad/s 

first-time  derivative  of 
roll 

rad/s 

first-time  derivative  of 
pitch 

rad/s 

first-time  derivative  of 
yaw 

ft/s 

initial  velocity  (VX)  - 
EAST 

ft/s 

initial  velocity  (VY)  - 
NORTH 

ft/s 

initial  velocity  (VZ)  - UI 

ft/s2 

initial  specific  force  in 
body  frame  (AX)  - EAST 

ft/s2 

initial  specific  force  in 
body  frame  (AY)  - NORTH 

ft/s2 

initial  specific  force  in 
body  frame  (AZ)  - UP 

(4)  Call-Lino  Data 


NAVLAT,  NAVLON,  NAW,  NAVH,  NAVP,  NATO,  NAVHD) 


(a)  Call-Line  Input 


Variable 

Units 

Data  Type 

T 

s 

REAL 

IENDF 

logical 

INTEGER 

DVI 

ft/s 

REAL 

ALTO 

ft 

REAL 

* 

DCM 

unity 

REAL 

Description 

current  simulation  time 

last-pass  indicator 

quantized  integral  of 
vehicle  specific  force  in 
the  inertial  frame 

indicated  altitude  from 
baroaltimeter 

direction  cosine  matrix 
(body  to  earth-fixed  frame 
transformation) 


(b)  Call-Line  Output 


Variable 

Units 

Data  Type 

Description 

NAVLAT 

deg 

REAL 

computed  navigational 

latitude 

NAVLON 

deg 

REAL 

computed  navigational 

longitude 

NAW 

ft/s 

REAL 

computed  navigational 
vector  in  ENU  frame 

velocity 

NAVH 

ft 

REAL 

computed  navigational 

altitude 

NAVP 

deg 

REAL 

computed  navigational 

pitch 

NAVR 

deg 

REAL 

computed  navigational 

roll 

NAVHD 

deg 

RFAL 

computed  navigational 

heading 

(5)  Formulation 

(a)  Initialization  Function 

The  initialization  switch,  INITSW,  is  initialized  to  zero  in  a 
DATA  statement.  The  following  functions  are  performed  on  the  first  pass, 
if  INITSW  = 0,  IENDF  = 0 , and  if  the  module  operating  cycle  time  has  elapsed. 
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• Read  and  print  initialization  file  (IFILE) . 

• Read  common  initialization  file  (PFILE) . 

• Set  OFILE  = XFILE. 

Initialize  for  vertical  damping. 

H = IH  + ALTERR 

HB  =•  ALTO 

OHB  = ALTO 

DELH  = HB  - H 

VDMP  = CVD3  * DELH  * DT 

NUP  = 0 

NORTH  * 4 


Add  errors  and  convert  units  of  latitude  and  longitude  to  radians 
INAVLA  = ILAT  + ( LATE RR/ RDTODG ) 

INAVLO  = I LON  + (LONERR/RDTODB) 
where  RDTODG  = 57.29577951  deg/rad 

The  initial  up,  east,  north  (UEN)  wander  angle  to  earth- fixed  trans- 
★ 

formation  matrix.  A,  is  computed  and  printed. 


P COSLT*COSLON J 

-CALF*S INION  - SALF*SINLT*COSLON  j 

SALF*SINLON  - CALF*S INLT*COSLON 

COSLT*SINLON  j 

COSLON*CALF  - SALF*SINLT*SINLON 

-SALF*COSLON  - CALF*SINLT*SINLON 

[_  SINLT  | 

SALF*COSLT  ! 

COSLT*CALF 

where 


SINLT 

SIN 

(INAVLA) 

COSLT 

COS 

(INAVLA) 

SINLON  = 

SIN 

(INAVLO) 

COSLON  = 

COS 

(INAVLO) 

SALF 

SIN 

(WONDER) 

CALF 

COS 

(WONDER) 
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Add  velocity  errors  to  velocity 


NAW  = IV  + VERR 


and  compute  initial  earth-relative  velocity  in  UfiNl-  wander-angle 
frame. 


V(l)  = NAW  (3) 

V (2)  = CALF  * NAW  ( 1 ) + SALF  * NAW  (2) 

V (3)  = -SALF*NAW  (1)  + CALF  *'NAW(2) 

Compute  the  mid  cycle  altitude,  XH 

XH  = H + 0.5  * V(l)  * DT 

Compute  the  initial  angular  velocity,  RHO,  of  the  UEN  wander  angle 
earth-fixed  frame  in  the  UEN-wander  angle  frame. 

Set  RHO (1)  = 0.0 

Call  angular  velocity  computational  subroutine  (Section  7b) 
CALL  ANGVEL  (V,  CALF,  SSLAT,  H,  RHO) 
where  the  input  variables  are 

V = initial  earth  relative  velocity  vector 
CALF  = COSINE  of  the  wander  angle 

SALF  = SINE  of  the  wander  angle 
SSLAT  = SINE  of  the  latitude  squared 
H = altitude 

and  the  quantity  output  is 

RHO  = angular  velocity  of  the  UEN-wander  angle  earth- 
fixed  frame. 

(See  Section  7b  for  subroutine  ANGVEL  description.) 
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Compute  computational  to  inertial  frame  rate  by  DT  and  coriolis 
corrections. 

Call  TORCOR  (A,  RHO,  V,  DT,  THG,  WXV) 

where  the  input  variables  are 
★ 

A = UEN-wander  angle  to  earth-fixed  transformation 
RHO  = initial  angular  velocity  in  UEN  WA  frame 
v = initial  earth  relative  velocity 

DT  s*  module  operating  cycle 

and  the  output  variables  are 

THG  « computational  frame  with  respect  to  inertial  frame 
rate  times  DT. 

WXV  * coriolis  correction 

(See  Section  7c  for  subroutine  TORCOR  description.) 

* * 

To  extrapolate  A to  mid  computation  cycle  and  restore  aa  XA 

first  define  THY  and  THZ,  the  Y and  Z components  of  the  angular  rate 
times  DT  at  mid  cycle. 

THY  - RHO (2)  * DT/2 

THZ  - RHO (3)  * DT/2 

Form  the  update  matrix,  H2DT 

CALL  AUP  (THY,  THZ,  H2DT) 
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and 


* * * 

CALL  MM  (A,  M2DT,  XA) 

* * 

to  update  the  transformation  matrix,  A,  to  XA  matrix.  (See  Section  7d 
for  subroutine  AtJP  description) . 

Extrapolate  the  wander  angle,  ALP,  to  mid  computation  cycle 

DALF  = (V (2)  * SINLT  * DT/2)/(RE  * CQSLT) 


where 


V{2)  » east  velocity  component 

SINLT  =>  SINE  of  the  latitude 
DT  = module  operating  cycle 
RE  » earth  radius 
COSLT  a COSINE  of  the  latitude 


and  calculate 


XSALF  » SALP  4-  DALF  * CALI-' 
XCALF  “ CALF  - DALF  * SALF 


Compute  gravity  at  mid  computation  cycle 
CALL  GRAV  (A,  XU,  XSSLAT,  GR) 

where  the  input  variables  are 

» 

XA  «=  mid  computation  cycle  UEN-WA  to  earth-fixed 
transformation  matrix 
XU  f the  mid  coaputation  cycle  altitude 
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and  the  output  variables  are 


XSSLAT  = SINE  of  the  latitude  squared  at  mid  computation 
cycle 

GR  = gravity  vector  at  mid  computation  cycle  in  the 
computational  frame 

(See  Section  7e  for  subroutine  GRAV  description) . 

Compute  the  coriolis  correction,  W XV,  at  mid  computation  cycle. 

CALL  TORCOR  (X*A,  RHO,  V,  DT,  THG,  WXV) 

where  the  quantities  are  described  above. 

The  quantized  integral  of  specific  force,  DVI,  is  initialized 
for  the  accumulation  at  the  algorithm  (ALG)  module  cycle  time. 

DVI  = (OoO,  0.0,  0.0) 

The  units  of  initial  latitude,  ILAT,  and  longitude,  ILON,  are  converted 
from  radians  to  degrees  and  restored  as 

I TEMPI  = ILAT  * RDTODG 
ITEMP2  = ILON  * RDTODG 
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The  initialization  switch  is  reset. 


INITSW  = 1 

and  the  simulation  time  incremented 

TLLN  = T + DT 

The  subroutine  returns  to  the  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating 

cycle. 


The  quantized  integral  of  specific  force,  DVI , is  transformed  from 
inertial  to  earth- fixed  coordinates  at  the  mid-computation  cycle. 


or 


DVE 


DVE 

CWET 

“SWET 

0 


Z (WET)  * DVI 
SWET  0 ] 
CWET  0 * 

0 1 


DVI 


where 

CWET 

SWET 

and  WET 


COS (WET) 

SIN (WET) 

WE  * (T  - DT/2) 


and  DVE  is  further  transformed  to  DVC  or  from  earth-fixed  to  computational 
frame  at  mid-computation  cycle. 

DVC  “ DVE  * XA 

The  velocity  from  the  previous  pass  is  restored  as  OV. 


OV  v 
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The  velocity,  V,  is  now  updated  in  the  UEN  frame  for  the  wander  angle  *»  0. 

V = 0V  + S-VC  + WXV  + GR  * DT 
and  a vertical  damping  term  is  added 

V = V + (VDMP  + CVD2  * DELH)  * DT 

The  velocity  and  its  value  on  the  previous  pass  are  averaged  to 
obtain  an  interpolated  value,  HV 

HV  = (V  + 0V)/2 

and  extrapolated  to  obtain  XV 

XV  = (3  * V - 0V)/2 

To  compute  the  angular  velocity,  RKO,  the  subroutine  ANGVEL  is  called, 
(see  Section  7b) . 

CALL  ANGVEL  (HV,  XCALF,  XSALF,  XSSLAT,  XH,  RHO) 
where  the  arrangements  are 

HV  = the  average  velocity 

XSALF  = SINE  of  the  mid-computation  cycle 
wander  angle 

XCALF  = COSINE  of  the  mid-computation  cycle 
wander  angle 

ASSLAT  = SINE  of  the  latitude  squared 
XH  = the  mid-computation  cycle  altitude 

RHO  = the  angular  velocity 

Using  the  angular  velocity,  RHO,  the  computational  to  earth  fixed 

★ 

transformation  matrix.  A,  is  calculated. 

First,  THY  and  THZ  arc  defined, 

THY  = RHO (2)  * DT 
THZ  «*  RHO (3)  * DT 

r 

l 
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r 


* * 

and  the  A matrix  update  subroutine  is  called  to  obtain  the  matrix,  M2DT 

CALL  AUP  (THY,  THZ,  M2DT) 

* 

The  matrix  multiplication  of  the  old  value  of  the  A - matrix  is 
* * 
multiplied  by  M2DT  to  obtain  the  matrix,  ATEMP 

* * * 

ATEMP  = A * M2DT 

★ * 

and  ATEMP  is  restored  as  the  updated  matrix  A 

A = ATEMP 

The  update  counter  initially  set  to  zero  is  incremented 

NUP  = NUP  + 1 

* 

The  orthonormalization  of  the  transformation  matrix.  A,  is  by  passed, 
when  the  update  counter,  NUP,  is  less  than  the  orthonormalization  parameter, 
NORTH 


IF  (NUP  .LT.  NORTH) 

On  an  orthonormalization  pass  , reset 

NUP  = 0 

* 

and  the  A transformation  ma  .rix  is  now  orthonormalized 

• • * * 

A = A - 1/2  * A * (A  A - I) 

• * * 

and  AV  , set  equal  to  A to  obtain  the  A matrix  in  row  vector  form. 

x 

* * 

N AV  = A 
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The  extrapolated  mid-computation  cycle  altitude  error  is  calculated. 


DELI!  = XHB  - XH 


from  the  error  between  the  extrapolated  barometric  and  the  computed  altitude. 


The  vertical  acceleration  error,  VDMP,  is 


VDMP  = VDMP  + CVD3  * DELH  * DT 


The  computed  altitude,  H,  is  stored  as  the  variable  NAVH 


NAVH  = H 


The  computed  latitude,  NAVLAT, 

2 2 1/2 

NAVLAT  = ARCTAN(A{3,1)/(A(1,1)  + A(2,l)  ) ' ) 


the  computed  longitude,  NAVLON, 


NAVLON  = ARCTAN(A(2,1)/A(1,1)) 

/ 

and  the  wander  angle/  ALF, 


AtF  = ARCTAN (A (3 , 2) /A(3 , 3) ) 

/ 

The  wander  angle/ from  the  previous  pass  is  denoted  as, 

/ 

/ 

/ OALF  = ALF 

/ 

i 

and  the  mid-computation  cycle  value  of  the  wander  angle  is, 

t 

{ 

r 

I DALF  = (OALF  + ALF)/2 

/ 

/ 

From  the  gander  angle,  ALF,  are  calculated, 

/ 

/ 

i CALF  = COS (ALF) 

k 

I SALF  » SIN (ALF) 

/ 

/ 
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Now  the  transformation  matrix.  A,  is  extrapolated  to  mid-computation 
cycle.  THY  and  THZ  are  set  equal  to  half  of  their  respective  value. 


THY  = THY/2 
THZ  = THZ/2 


* * 

and  the  A matrix  update  subroutine  is  called  to  obtain  the  M2DT02  matrix. 

(Section  7d) 

CALL  AUP  (THY,  THZ,  M2DT02) 

★ 

The  updated  transformation  matrix,  XA,  is  formed  in  the  matrix  multi- 
plication , 

XA  = A * M2DT02 

An  updated  altitude,  H,  is  calculated  from  the  mid-computations  cycle 
vertical  velocity  and  the  error  in  the  extrapolated  values  of  the  calculated 
altitude  and  the  barometric  altitude. 

H = H 4 (HV(1)  + CVDl  * DELH)  * DT 

An  extrapolated  altitude  XI!  is  calculated 

XH  = H + V(l)  * DT/2 

The  barometric  altitude,  ALTO  is  redefined  as  HB 

HB  = ALTO 


and  the  extrapolated  barometric  altitude  is  calculated 

XHB  = (3*HB  - 0HB)/2 

where 

OHB  = HB,  the  barometric  altitude  on  the 
previous  pass 
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and  the  extrapolated  values  of  CALF  and  SALF  are  calculated  from 

XSALF  = SALF  + DALF  * CALF 
XCALF  = CALF  - DALF  * SALF 


and  the  computed  velocity  vector,  NAW,  is  tranformed  to  ENU  fram 
or 


NAW  = 


NAW 

CALF 

SALF 

0 


= Z(-ALF)  * V 
-SALF  0 
CALF  0 
0 1 


V 


Gravity,  GR,  extrapolated  to  mid-computation  cycle  is  calculated  by  the 
subroutine, 


CALL  GRAV  (XA,  XH,  XSSLAT,  GR) 

★ 

where  XA  = the  mid-computation  cycle  extrapolated 

transformation  matrix 

XH  = The  mid-computation  cycle  extrapolated 
altitude 

XSSLAT  = The  mid- computation  cycle  extrapolated 
squared  sina  of  the  latitude 

GR  = the  gravity  vector  at  mid  comp  cycle. 

The  angular  velocity,  RHO,  is  calculated  by  the  subroutine 


CALL  ANGVEL  (XV,  XCALF,  XSALF,  XSSLAT,  XH,  RHO) 

where  XV,  XCALF,  XSALF,  XSSLAT,  and  XH  arc  quantities  extrapolated  to 
mid-computation  cycle. 

Using  the  angular  velocity,  RHO,  the  subroutine  TORCOR  is  called  to 
calculate  the  vector,  THG,  which  defines  the  computation  to  inertial  frame 
rates  times  DT. , and  the  coriolis  corrections,  WXV,  at  mid-computation  cycle. 

CALL  TORCOR  (XA,  RHO,  XV,  DT,  THG,  WXV) 


2-166 


The  quantized  integral  of  specific  force,  DVI,  is  reinitialized  for 
the  accumulation  at  the  algorithm  (ALG)  module  cycle  time. 

DVI  = (0.0,  0.0,  0.0) 


The  body  to  computation  frame  transformation  (attitude)  matrix, 

* 

DTEM,  is  calculated 


★ fctV  * 

DTEM  = AV  * DCM 


The  computed  attitude  angles  are  extracted.  First  the  computed  pitch 
angle,  NAVP 


where 


DEN 


NAVP  = ARCTAN (DTEM (1) /DEN) 

( DTEM ( 2 ) 2 + DTEM(3)2) 1>/2 


The  computed  roll  angle,  NAVR, 

N AVR  = ARCTAN ( DTEM ( 2 ) /DTEM ( 3 ) ) 
and  the  computed  heading,  NAVHD, 

NAVHD  = ARCTAN (DTEM (4) /DTEM ( 7) ) - ALF 

If  DEN  = 0.0,  the  pitch  is  ±90°,  the  previous  attitude  values  are 
output,  the  simulation  time  is  incremented,  TLLN  = T + DT,  and  the  sub- 
routine returns  to  the  main  program., 

Otherwise  the  computed  attitude  and  position  values  are  converted 
from  radians  to  degree s. 
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ITEMP1 

= 

NAVLAT 

* RDTODG 

ITEMP2 

= 

NAVLON 

* RDTODG 

ITEMP3 

NAVP  * 

RDTODG 

ITEMP4 

= 

NAVR  * 

RDTODG 

ITEMP5 

= 

NAVHD  * 

' RDTODG 

and  NAVH,  NAW,  ITEMP1,  I TEMP  2 , ITEMP3,  I TEMP  4,  ITEMP5,  A and  DTEM  are 
printed,  and  the  simulation  time  incremented 

TLLN  = T + DT 

The  subroutine  returns  to  the  main  program. 

(6)  Output 

(a)  Print 

FORTRAN  unit  numbers  OFILE  = 6. 

On  the  initialization  pass  the  title  “NAVIGATION  INITIALIZATION" 
and  the  initialization  data  are  printed. 

Printed  output  is  produced  at  PRNTDT  or  MODPDT  intervals  when 
PRNTSW  ^ 1.  See  Section  2.2  for  print  control  logic.  The  printed  output 
is  as  follows, 


Variable 

Units 

Description 

NAVH 

ft 

computed  navigational  altitude 

NAW 

ft/s 

computed  navigational  velocity  vector 
in  ENU  frame 

NAVLAT 

deg 

computed  navigational  latitude 

NAVLON 

deg 

computed  navigational  longitude 

NAVP 

deg 

computed  navigational  pitch 

NAVR 

deg 

computed  navigational  roll 
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Variable 

Units 

Description 

NAVHD 

deg 

computed  navigational  heading 

* 

A 

unity 

the  UP,  east,  north  wander  angle 
(Computational)  frame  to  earth-fixed 
transformation  matrix 

DTEM 

unity 

the  body  to  computation  frame 
(attitude)  transformation  matrix. 

t, 


(7)  Subroutines  Called 


(a)  MTXM  - matrix  transpose  by  matrix  multiplication,  (See  Vol.  Ill 
Section  2,3.15) 


(b)  Angular  Velocity  (ANGVEL) 

This  subroutine  computes  level  components  of  the  vehicle  angular  velocity 
due  to  its  motion  with  respect  to  the  earth. 


Call  Line  Data 


INPUT 

AVEL,  ACALF,  ASALF,  ASSL,  ALT, 


RHO 

OutputT 


Call  Line  Input 


Variable 

Data  Type 

Description 

AVEL 

REAL 

earth  relative  velocity  vector 

ACALF 

REAL 

COSINE  of  the  wander  angle 

ASALF 

REAL 

SINE  of  the  wander  angle 

ASSL 

REAL 

SINE  of  the  latitude  aquared 

ALT 

REAL 

altitude 

Call  Line  Output 


Variable 


Data  Type 


Description 


RHO 


REAL 


angular  velocity  vector  in 
UEN  - WA  frame 


Formulation 


Compute  the  east  and  north  components  of  the  earth  relative  velocity 
in  the  UEN  - WA  frame 

VE  = ACALF  * AVEL  (2)  - ASALF  * AVEL  (3) 

VN  « ASALF  * AVEL  (2)  + ACALF  * AVEL  (3) 

Calculate  the  radii,  RM  and  RP,  of  curuature  of  the  WGS-72  earth  model  for 
the  meridional  plane  (N-S)  and  the  prime  vertical  plane  (E-W),  respectively, 

RP  « ALT  + RE/U-ESQ  * ASSL) 

3/2 

RM  ■ ALT  + RESQ/< 1-ESQ  * ASSL)  ' 

where 

RE  “ 2.0925640E7,  the  earth's  radius. 

ESQ  a 0.006694317778,  the  square  of  the 

eccentricity  of  the  earth's  meridional 
ellipse. 

RESQ  » 2.078555712E7,  =»  RE  * (1-ESQ) 

Now  the  east  and  north  components  of  the  angular  rate,  WE,  and  WN, 
are  calculated 

WE  ■ -VN/RM 
WN  « VE/RP 

and  are  converted  to  the  angular  velocity,  RHO,  of  the  UEN-WA  frame  with 
respect  to  the  earth- fixed  f reuse. 
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RHO(l)  = 0.0 

RHO(2)  = ACALF  * WE  + ASALF  * WN 
RHO (3)  = -ASALF  * WE  + ACALF  * WN 

(See  flow  chart,  Figure  2). 

(c)  Angular  Velocity  by  DT  and  Coriols  Corrections  (TORCOR) 

This  subroutine  computes  computational  to  inertial  frame  angular 
velocities  times  DT  and  the  coriolis  corrections. 

Call  Line  Data 


INPUT 


A,  RHO,  V,  DT, 


THG,  WXV 


Call  Line  Input 


Variable 

Data  Type 

Description 

* 

A 

REAL 

UEN-WA  earth-fixed  transformation 

RHO 

REAL 

angular  velocity  vector  in  UEN-WA  fre 

V 

REAL 

earth  relative  velocity  vector 

DT 

REAL 

module  operating  cycle 

Call 

Line  Output 

variable 

Data  Type 

Description 

THG 

REAL 

Computational  frame  with  respect 
to  inertial  frame  angular  rate  times 
DT 

WXV 

REAL 

coriolis  correction 

CALCULATE 
NORTH  & EAST 
VELOCITY 
COMPONENTS 


I 


r CALCULATE  1 

EARTH  PRIME 

VERTICAL  (Re 

)& 

MERIDIONAL  ( 

Rm) 

CALCULATE 

NORTH  & EAST 
ANGULAR  VELOCITY 
COMPONENTS 

1 

l 

! CALCULATE 

ANGULAR 

RHO 

VELOCITYj 

' \ 

r 

^ RETURN  ^ 


Figure  2.  Angular  velocity  computation  module  (ANGVEL) 


Formulation 


Calculate  the  earth  rate  signals,  EAR,  in  the  computational  frame. 

EAR(l)  = A(3,l)  * WE  * DT 

EAR (2)  = A(3 ,2)  * WE  * DT 

EAR (3)  = A(3 ,3)  * WE  * DT 

where  WE  = 0.7292115147E-5  rad/sec,  the  earth  rotation  rate. 

The  gyro  torquing  signals,  THG,  are 

THG  = EAR  + RHO  * DT 
and  the  angular  velocity  (for  wxv),  THE, 

THE  ® THG  + EAR 

Compute  the  vector  cross  product  of  the  angular  rate  and  the  vehicle 
velocity  to  obtain  the  coriolis  correction,  WXV,  for  the  velocity  update. 

WXV  o THE  X V 

(See  flow  chart,  Figure  3), 

(d)  PCM  Second  order  Update  Matrix  (AUP) 

This  subroutine  forms  the  second  order  update  matrix  for  the  local 
vertical-wander  angle  computational  to  earth  fixed  frame  direction  cosine 
matrix  (DCM) . 

Call  Line  Data 

INPUT 
DV,  D5S, 

MUP 

OUTPUT 
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Figure  3.  Angular  velocity  times  DT  and  Coriolis  Correction 
Module  (TOKCOR) 


Call  Line  Input 


Variable  Data  Type 

DY  REAL 

DZ  REAL 

Call  Line  Output 

Variable  Data  Type 

MUP  REAL 


Description 
Y-axis  increment 
Z-axis  increment 

Description 

Second  order  update  matrix 


Formulation 


Calculate  the  second  order  update  matrix, 


HUP 


MUP  « 


l-(DYa  + 0Z2}/2 
DZ 
-DY 


I t 

I -DZ  I DY 

| l-DZ2/2  j DY  * DZ/2 

j DY  * DZ/2j  l-DYJ/2 


(See  flow  chart,  Figure  4). 
I®)  Gravity  Computation  (GRAV) 


This  subroutine  computes  three  components  of  gravity  in  the  computational 
frame  using  the  WGS-72  ellipsoidal  earth  modal. 


Call  Line  Data 

INPUT 
AH,  ALT, 

SSL,G 

OUTPUT 
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DEFINE 

ELEMENTS 

OF 

UPDATE  MATRIX 

...I  ■ . 


Figure  4.  Update  for  coeducation  to  earth  fixed  transformation 
matrix  module  (AUP) 
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Call  Line  Input 


Variable 

Data  Type 

Description 

* 

AM 

REAL 

computational  frame  transformation 
matrix 

ALT 

REAL 

altitude 

Call  Line 

Output 

Variable 

Data  Type 

Description 

SSL 

REAL 

AM(3,1)  squared 

G 

REAL 

gravity  vector 

Formulation 

• - . 

Define 

SSL  « AMP, l)2 

- 

and 

COEF  » 1.63B-8 

* ALT  * AM(3,1) 

Calculate  the  three  components  of  gravity/  G,  in  the  computational 
frame. 

0(1)  * -(32.0677057  + 0.16939081  * SSL  + 7, 5281E-4  * SSL2) 

*(1-(9.622?F  -8-S-5-6.  408E-10  * SSL)  * ALT 
+ 6.S512E-15  * ALT2) 

0(2}  » COEF  * AM  (3,2) 

0(3)  « COEF  * AM  (3,3) 


(See  flow  chart,  Figure  5) 


CALCULATE 

GRAVITY 


COMPONENTS  IN 
COMP  FRAME 


Figure  5,  Gravity  computation  Modulo  (GRAV) 


(f)  Matrir  Multiply  (MM) 


This  subroutine  forms  the  product  of  two  3x3  matrices. 


Call  Line  Data 


INPUT 

* * " 

A , B, 

* 

C 

OUTPUT 


Call  Line  Input 


Variable 

Data  Type 

Description 

* 

A 

REAL 

first  matrix 

* 

B 

REAL 

second  matrix 

Cali  Line  Output 

Variable  Data  Type  Description 

* 

c product  of  input  matrices 

Formulation 

Compute  matrix  products,  c 

* * * 

C - A * D 

(Soe  flow  chart,  Figure  6), 
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2.3.14  EVALUATION  MODULE  (EVL) 


(1)  General  Description 

This  module  was  included  to  provide  the  user  with  a means  of  per- 
forming an  error  analysis  of — or  otherwise  manipulating — the  INSS  tra- 
jectory and  navigation  data  during  a simulation,  or  collecting  simula- 
tion data  for  post-run  analysis.  The  EVL  module  simply  computes  and 
prints  a table  of  navigation  and  attitude  errors  and  trajectory  param- 
eters every  50  module-operating  cycles.  The  printing  frequency  is  based 
on  the  user- specified  operating-frequency  time  (DT). 

(2)  Input 


(a) 

Module  Initialization 

File 

(IFILE) 

FORTRAN  unit 

numbers 

90 

FORTRAN  formats  15, 

F20.10 

Index 

Variable 

Default 

Value 

Units 

Description 

1 

DT 

1.0 

3 

modulo  operating  cycle  time 

2 

WttJTSW 

1.0 

logical 

print  switch  (0-noprinfc, 
otherwise  print) 

3 

oursw 

0,0 

mt 

not  used 

4 

XFILE 

6.0 

logical 

FORTRAN  unit  number  for 
printout 

5 

PRNTDT 

1.0 

s 

printing  cycle  time 

(b) 

Common  Initialization 

Fi  lo 

(PFXLE) 

FORTRAN  unit 

numbers 

7 

FORTRAN  formats  15, 

F20, 10 

Index 

Variable 

Default 

value 

Units 

Description 

1 WE  0.7292115147E-4  rad/s  earth  rotation  rai 

2 2092 5640. 0 ft  earth  radius 

3 G 32.2  ft/s  nominal  gravity 
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(3)  Call-Line  Data 


INPUT 


T,  IBNDF,  LAT,  ION,  ALT,  VEL,  DVT,  PITCH,  ROLL,  YAW,  WONDER^NAVLA^ 


INPUT 


NAVLON,  NAW,  NAVH,  NAVP,  NAVR,  NAVHO) 


(a)  call- 

-Line  Input 

Variable 

Units 

Data  Type 

T 

s 

REAL 

IENDF 

logical 

REAL 

LAT 

rad 

REAL 

LON 

rad 

REAL 

ALT 

ft 

REAL 

VEL 

ft/a 

REAL 

DVT 

ft/s 

REAL 

PITCH 

rad 

REAL 

ROLL 

rad 

REAL 

YAW 

rad 

REAL  - 

WONDER 

rad 

REAL 

NAVLAT 

rad 

REAL 

NAVLON 

rad 

REAL 

UAVH 

ft 

REAL 

Description 

current  simulation  time 

last-pass  indicator 

geodetic  latitude  - "true** 
geodetic  longitude  - "true" 

altitude  above  sea  level  - 
"true" 

velocity  vector  in  body  frame  - 
“true* 

quantized  integral  of  specific 
force  in  ENU  frame  - *true" 

vehicle  pitch  - ••true" 

vehicle  roll  - “true** 

vehicle  yaw  - "true** 

vehicle  wander  angle  - "true" 
(clockwise  from  north  about 
the  local  vertical  up  axis) 

computed  navigational  latitude 

computed  navigational  longitude 

coeluted  navigational  altitude 
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Variable 

Units 

Data  Type 

Description 

NAVP 

rad 

REAL 

computed  navigational  pitch 

NAVR 

rad 

REAL 

computed  navigational  roll 

NAVHD 

rad 

REAL 

computed  navigational  heading 

(b)  Call-Line  Output 

There  are  no  output  parameters  in  the  call-line  argument  list. 
(4)  Formulation 

(a)  Initialization  Function 

The  initialization  switch,  INITSW,  is  initialized  to  zero  in  a DATA 
statement.  The  following  functions  are  performed  on  the  first  pass,  if 
INITSW  = 0,  IENDF  '■  0,  and  if  the  module  operating  cycle  time  has  elapsed. 

• Read  and  print  initialization  file  (IFILE), 

• Read  common  initialization  file  (PFILE) 

• Set  OFILE  a XFILE. 

The  initialization  switch  is  set,  INITSW  a i,  and  the  current  simu- 
lation time  is  incremented, 

TEVL  * T + DT 

Now  the  subroutine  returns  to  tha  main  program. 

(b)  General  Function 

The  following  functions  are  performed  every  module  operating  cycle. 

Tho  13  trajectory  and  the  9 navigation  error  parameters  are  arrayed 
in  SO-elcment  vector  form  and  the  unit  converted  to  units  convenient  for 
interpretation. 


2-183 


XT(N) 

3 

T 

(s) 

XLAT(N) 

a 

LAT  * RDTODG 

(rad  to  deg) 

XLQN(N) 

3 

LON  * RDTODG 

(rad  to  deg) 

XALT(M) 

ALT 

(ft) 

XVELX(N) 

a 

VEL(l) 

(ft/s) 

XVELY(N) 

a 

VEL(2) 

(ft/s) 

XVELZ(N) 

a 

VEL(3) 

(ft/9) 

XDVTX(N) 

a 

DVT(l) 

(ft/s) 

XDVTY(N) 

a 

DVT (2) 

(ft/s) 

XDVTS(N) 

o 

DVT(3) 

(ft/s) 

XKEAD(N) 

n 

(YAW-WONDER)  * RDTODG 

(rad  to  deg) 

XPXTCU(N) 

M 

PITCH  * RDTODG 

(rad  to  deg) 

XRQLL(N) 

a 

ROLL  * RDTODG 

(rad  to  deg) 

The  navigation  errors  are  the  differences  betwoen  the  "true1*  position 
and  attitude  values  and  tho  corresponding  computed  navigational  values  in 
their  appropriate  units. 


ELAT(N) 

m 

(LAT-NAVLAT) *RE 

(rad  to  ft) 

ELON(N) 

'a 

(LON-WAVLON)  *S1E*C0S  (LAT) 

(rad  to  ft) 

EALT(N) 

m 

ALT  - NAVH 

(ft) 

EVELX(N) 

m 

VEL(l)  - NAW(l) 

(ft/s) 

EVELV(N) 

ts 

VEL(2)  - NAW(2) 

(ft/s) 

EVELZ(N) 

w 

VEL(3)  - NAW(3) 

(ft/s) 

EHEAD(N) 

3 

(YAW  - WONDER  - NAVHD)  *36<30*RDTODG  (r 

EPTTCH(N) 

m 

(PITCH  - NAV) *36Q0*RDTODG 

(rad  to  sec) 

DROLL (N) 

m 

(ROLL  -•  NAVR)  •3600*RDTODB 

(rad  to  sec) 
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The  listed  trajectory  and  navigation  error  parameters  are  coeluted 
and  stored  for  N = 50  module  operating  cycles.  When  the  50  lines  of 
trajectory  and  the  50  lines  of  navigational  error  values  are  collected, 
each  of  these  pages  of  data  is  then  printed  and  written  on  an  unformatted 
tape  or  disc  file. 

The  simulation  time  is  incremented 

TEVL  = T + DT 

The  subroutine  now  returns  to  the  main  program. 

(5)  Output 

(a)  Print 

FORTRAN  unit  numbers  QFILE  « 6. 

On  the  initialization  pass  the  title  "EVALUATION  INITIALIZATION"  and 
the  initialization  data  are  printed. 

A printed  output  table  is  produced  every  50  module  operating  cycles 
or  on  last  pass.  Thu  remaining  data  is  printed,  if  the  print  switch, 
PRNTSW  £ 1.  A sample  of  the  printed  output  is  displayed  in  Section  2,2. 
The  printed  output  consists  of  the  following  list  of  variables. 


Variable 

Units 

Description 

XT 

s 

current  simulation  time 

XLAT 

deg 

geodetic  latitude  - "true" 

x£on 

deg 

geodetic  longitude  - "true" 

XALT 

ft 

altitude  above  sea  level  - "true" 

XVELX 

ft/s 

east  velocity  component  - "true" 

XVELV 

ft/s 

north  velocity  component  - "true* 

XVELZ 

ft/s 

up  velocity  component  - "true" 

XHEAD 

deg 

heading  - "true" 
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Variable 

Units 

Description 

XP1TCH 

deg 

pitch  - "true" 

XROLL 

deg 

roll  - "true" 

ELAT 

ft 

latitude  error  (“true"  - computed) 

ELON 

ft 

longitude  error  ("true"  - computed) 

EALT 

ft/s 

altitude  error  ("true"  - computed) 

EVELX 

ft/s 

east  velocity  error  ("true"  - computed) 

EVELV 

ft/s 

north  velocity  error  (“true"  - computed) 

EVELZ 

ft/s 

up  velocity  error  ("true"  - computed) 

EHEAD 

s 

heading  error  (“true"  - computed) 

EPITCIl 

s 

pitch  error  ("true"  - computed) 

ERQLL 

s 

roll  error  ("true"  - computed) 

(b)  Secondary  Storage 

(PPFILE) 

FORTRAN 

unit  number : 12 

FORTRAN 

formats 

unformatted 

The  following  output  is  written  on  tape  or  disc  on  every  modulo  cycle 
for  later  postprocessing. 

The  variable  list  is  identical  with  the  printed  list  above. 

(7)  Subroutines  Called 

No  subroutines  are  called. 
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2.3.15  MATHEMATICAL  SUBROUTINES 


(1)  General  Description 

The  INSS  sytem  includes  a library  of  three  dimensional  mathematical 
subroutines  that  perform  vector-matrix  operations  and  r andean  number  generation. 
All  vectors  must  be  dimensioned  V{3).  All  matrices  must  be  dimensioned  M(9) 
and  stored  sequentially  by  row,  i.e.. 


Mu 

M12 

M13 

M(l) 

M(2) 

M(3) 

* 

M a 

M21 

M22 

M23 

a 

M(4) 

M(5) 

M(6) 

«31 

M32 

M33 

M(7) 

M(8) 

M(9) 

(2)  Subroutines 

The  following  subroutines  are  described. 

(a)  Matrix  Multiplication  (MXM) 

This  subroutine  multiplies  two  9-element  matrices. 

Call  Line  Data 
INPUT 

(Ml,  M2, 

M3) 

OUTPUT 


call  Line 

Input 

Variable 

Data  Type 

Description 

* 

HI 

REAL 

A 3 x 3 matrix  stored  by  row  as 

a 9-eloaont  vector 

<1 

M2 

REAL 

A 3 x 3 matrix  stored  by  row  as 
a 9-eletaent  vector 
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Call  Line  Output 


Variable  Data  Type  Description 


* 

M3  SEAL  A 3 x 3 matrix  stored  by  row  as 

a 9-element  vector 


Formulation 

* * * 

The  matrices  Ml  and  M2  are  multiplied  and  stored  in  matrix  M3 


M3  = Ml  * M2 


(b)  Matrix  by  Vector  Product  (MXV) 

A matrix  time  vector  product  is  produced  to  obtain  a vector. 


Call  Line 

Data 

INPUT 

* 

(H,  VI, 

V2) 

OUTPUT 

Call  Line 

Input 

Variable 

Data  Type 

Description 

it 

N 

REAL 

A 3 x 3 matrix  stored  by  now  as 
nine  element  vector 

vT 

REAL 

A 3 element  vector 

Call  Line 

Output 

Variable 

Data  Type 

Description 

V2 

REAL 

A 3 element  vector 
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Formulation 


* 

The  matrix,  M,  is  multiplied  by  the  vector,  vj  and  stored  as  the 

vector,  V2. 

V2  = M * VI 


(c)  Matrix  Transpose  by  Matrix  Product  (MTXM) 

A transposed  matrix  is  multiplied  by  another  matrix  to  obtain  a 
third  matrix. 

Call  Line  Data 


INPUT 


OUTPUT 


Call  Line  Input 


Variable  Data  Type 


Description 


* 

Ml  REAL  A 3 x 3 matrix  stored  by  row  as 

a nine  element  vector 

* 

M2  REAL  A 3 x 3 matrix  stored  by  row  as 

a nine  element  vector 


Call  Line  Output 
Variable  Data  Type 

M3  REAL 


Description 

A 3 x 3 matrix  stored  by  row  as 
a nine  element  vector 
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Formulation 


* * 

The  transpose  of  matrix,  Ml,  is  multiplied  by  the  matrix,  M2,  and 
* 

stored  as  the  matrix,  M3. 

* * * 

M3  = Ml  * M2 

(d)  Matrix  Transpose  by  Vector  Product  (MTXV) 

A transpose  matrix  is  multiplied  by  a vector  to  obtain  a vector. 

Call  Line  Data 


INPUT 
(M,  VI, 

V2 

OUTPUT 


Call  Line  Input 


Variable 

* 

M 

vT 


Data  Type 

REAL 

REAL 


Call  Line  Output 

Variable  Data  Type 

V2  REAL 


Description 

A 3 x 3 matrix  stored  bv  row  as 
a nine  element  vector 

A 3 element  vector 


Description 


A 3 element  vector 


Formulation 

* — 

A transpose  matrix,  M,  is  multiplied  by  a vector,  VI,  and  stored 

as  the  vector,  V2. 

— — 

V?  = M * VI 


2-190 


(e)  Random  Number  Generator  (GAUSS) 

This  function  computes  a Gaussian  random  number  with  a given  mean 
and  standard  deviation.  It  is  machine  independent  and  will  work  properly 
if  the  single  precision  word  of  the  machine  is  more  than  20  bits  long. 

Call  Line  Data 

INPUT 
(MEAN, STD) 

Call  Line  Input 

Variable  Data  Type  Description 

STD  REAL  desired  standard  deviation  of  the 

normal  distribution 

MEAN  REAL  desired  mean  of  normal  distribution 

Formulation 

An  odd  integer  seed  is  selected,  initially,  as  IX  = 3.  A real  number, 

FA,  is  initialized  to  zero. 

Twelve  uniformly  distributed  numbers,  FY,  from  0 to  +1  are  calculated. 

Those  numbers  are  summed  to  obtain  the  real  gaussian  random  number, 

FA,  which  has  a mean  of  6 and  standard  deviation  of  1.0. 

The  real  random  number  GAUSS,  returned  by  the  function  is  from  a 
Gaussian  dstribution  having  a mean  value  of  MEAN  and  a standard  deviation 
of  STD.  It  is  calculated  from 

GAUSS  = MEAN  + STD  * (FA  - 6.) 
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SECTION  3 


SAMPLE  OUTPUTS 


The  following  sample  output  has  been  computed  for  the  default 
values  in  the  initialization  files. 

3-1  Initialization 

These  pages  of  printout  list  each  of  the  module 
initialization  data  used  in  this  run. 
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1‘iUS  PALffi  IS  BEST  QUALITY  imdlAA&f/E 
7&M  COPY  fulfil  SHI®  TODDC 


6/23/78  SAMPLE  PUN  WITH  DEFAULT  DATA 


PHYSICAL  OATA  FILE 

HE  .72921151D-04 

20925640 . 
g 32.200000 

PPN10T  6.00C0000 


rao/sec 

ft 

FT/SEC2 

SECS 


SEQUENCER 

0T 

FRMTSU 
O'JT  HSU 
OFILE 

tpntdt 

rl'-OFOT 

TEK'J 

OTSLOU 


INITIALIZATION 
. 10000C00 
1. 0000000 
.0 

6 

6.0000000 

6.0000000 

24.000000 

.20000000D-01 


SEC 


trajectory 

0T 

FPNfSW 

O'JTTN 

OFILE 

f.Cn?0T 

fSNTOT 


INITIALIZATION 

.10000000 

1.0000000 

0 

6 

6.0000000 
6 . 0000000 


SEC 


lAT(DCG) 

LCN1DEG) 

ALT(FT) 

VEL( FT/SEC) 


40.000000 

.0 

2000.0000 
1000.0000 


1000.0000 


100.00000 


ROLL(DEGS) 

PITCH10EGS1 

yainoegsj 

WAKIEtUOEGSl 
AQ( FT/5CC2 ) 
W:-(  RADS/SEC) 


.0 

10.000000 

90.000000 

45.000000 

-5.5030645 

.536603070-04 


.113507370-04 

.107115650-03 


-31.663196 

.787563530-04 


ENVIRONMENT 

OT 

PRNTSW 

OUTSW 

OFILE 

P?*!t0T 

li.Ci  JT 

VISSW 


INITIALIZATION 

.10000000 

1.0000000 

0 

6 

6.0000000 

6.0000000 

.0 


SEC 


VIERATION  GENERATOR 

VERTICAL  LOAD 
4 PEAKS 


parameters 

AP 

.136031000-03 


WH  WO 

3.0000000 


13.500000 
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THIS  PAGE  IS  BEST  QUALITY PBAC^MA&iB 
PROH  COPY  FURNISHED  TO  DD.C  


.15488000D-04 

3.0000000 

.16150000D-05 

6.0000000 

.17500000D-07 

4.0000000 

LATERAL  LOAD 

3 PEAKS 

.103040000-04 

3.5000000 

.59000000D-07 

8.0000000 

. 30S00000D-C8 

6.0000000 

LONGITUDINAL  LOAD 
0 PEAKS 


21.000000 

32.300000 

61.500000 


16.000000 

36.000000 

68.000000 


PITCH  RATE 
3 PEAKS 

.26200000D-06  3.3000000 

.660685000-07  2.0000000 

.132260000-07  6.0000000 

YAW  RATE  PAD 
6 PEAKS 

.261902500-06  3.0000000 

.260015000-07  5.0000000 

. 23S5000CD-09  9.0000000 

.610000000-10  3.0000000 

ROLL  RATE  PSD 
1 PEAKS 

.720000000-06  2.0000000 


13.500000 

21.000000 

32.300000 


3.0000000 

15.000000 

33.000000 
67.800000 


2.0000000 


INITIALIZATION 

.100000000-01  SEC 
1.0000000 
6.0000000 
6.0000000 

0 

6 


GYROSCOPE 

OT 

PRNTSH 

NPDPDT 

FRNTDT 

OUTSW 

OFILE 


QGBX  s 


QGBY  o 


QCB2  a 


QUANT 


K 

I 

DELI 


1.0000000 

.0 

.0 

.0 

1.0000000 

.0 

.0 

1.0000000 

.0 


.0 

.0 

-1.0000000 

1.0000000 

.0 

.0 

.0 

.0 

1.0000000 


.0 

1.0000000 

.0 

.0 

.0 

-1.0000000 

1.0000000 

.0 

.0 


.700000000-04  ARCSEC 


.310000000410  6M  Clt«*<2/SEC«*2 

226.00000  OH 

14.000000  GH  CH**2 
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CO 

7000000.0 

CYME  CM/RAD/SEC 

H 

151000.00 

GM  CM-"*2/SEC 

V, 

r 

z 

KI 

1.1200000 

1.1200000 

1.1200003 

OEG/HR/G 

KO 

-.320300000-01 

-.32090000D-01 

-.320000000-01 

DEG/HVG 

K5 

-.62000000 

-.62000000 

-.62000000 

0 EG/HP/6 

KII 

.0 

.0 

.0 

OE$/HP/G**2, 

KS3 

-.300000000-01 

-.300000000-01 

-.300000000-01 

OEG/HR/G'' <*2. 

KIO 

.0 

.0 

.0 

DEG/K9/G**2. 

KI3 

.21000000 

.21000000 

.21000000 

CE3/HR/6«"2i 

K03 

-.750000000-02 

-.750033300-02 

-.750000000-02 

CE3/KS/3li’'0i 

DIAS 

-.31000000 

-.31300330 

-.SlCOOCOO 

DES/HR 

C'T.'.SV 

.0 

.0 

.0 

(OtG/HP)*^ 

srro  , 

-20.030000 

-20.000000 

-20.000000 

FFfl 

OFMO  , 

-54.000000 

-54.000300 

-54.000000 

PFM 

srpi  , 

.0 

.0 

.0 

PFM/P AD/SEC 

or, mi  , 

.0 

.0 

.0 

PFM/R AO/SEC 

TPAM31 

.0 

.0 

.0 

OcG/HR 

TP.'MTC 

200.00000 

200.00000 

200.00000 

SEC 

DIASTC 

40.000000 

SEC 

ORDER 

0 

ACCELEROMETER 

INITIALIZATION 

OT 

. 10000C03D-01 

SEC 

f"?MTSU 

1.CCCC200 

i;o:ifor 

6.C3002C0 

FPNIDT 

6.0303300 

ovtci 

0 

OFILE 

6 

WAX  a 

1.0303000 

.0 

.0 

.0 

.0 

1.0000000 

.0 

•1.0000000 

.0 

Q3AY  a 

.0 

1.0000000 

.0 

1.0090000 

.0 

.0 

.0 

.0 

-l.ococooo 

CDAZ  a 

.0 

.0 

1.0000900 

1.0000000 

.0 

.0 

.0 

1.0000000 

.0 

I 

5.0000000 

C-M  CM»**2 

DELI 

.0 

GM  CM.“»S 

CO 

25000.000 

DYNE  CM/RA0/5EC 

NRC 

.70039090 

CM  CM 

CHANT 

1.C020000 

CM/OEC 

K 

.103009020*09  GM  CM**:/RA0  SEC"»2 

X 

Y 

z 

KO 

.0 

.0 

.0 

MXCOO  G/G 

KP 

.0 

.0 

.0 

MICRO  G/G 

Kir 

.00000900 

.20000000 

.03000900 

MICRO  G/5fc*2 

KPP 

.0 

.0 

.0 

MICRO  G/G442 

KIO 

.0 

.9 

.0 

MICRO  G.'G**»2 

KIP 

.0 

.0 

.0 

Micro  C-'G»«2 

KOP 

.0 

.0 

.9 

MICRO  G/6*fcC 

DIAS 

900.00000 

409.09900 

400.00000 

MICRO  G 

GU3V 

.0 

.0 

.0 

< MICRO  GM*»C 

SFGPO 

10.000009 

10.000030 

10.000900 

PPM 
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SFGMO 

10.000000 

SFGP1 

.0 

SFGM1 

.0 

RX 

1.0000000 

RY 

.90000000 

RZ 

.90000000 

ORDER 

0 

10.000000  10.000000  PPM 

•°  .0  PPM  /G 

•0  .0  PPM  /G 

■°  .0  FT 

.20000000  .0  FT 

•0  .20000000  FT 


ALTIMETER  INITIALIZATION 

OT  .100000000-01  SEC 

PRNTSW  1.0000000 

OUTSN  0 

OFILE  6 

PRNTDT  6.0000000 

NCOFDT  6.0000000 

NOIS3W  0 


ALTIMETER  UNCERTAINTIES 


TC 

UOfFT-2) 
U1ISEC4/FT2) 
UC(  FT2 ) 

U3 

U4< FT2 ) 


60.000000 

. 39999999E-13 

•22999993E-07 

17000.000 

.6C499933E-09 

97.000030 


READER  INITIALIZATION 


DT 

.99999979E-02 

PRNTSU 

1.0000000 

OUTSN 

.0 

OFILE 

6 

NOD TOT 

6.0000000 

PBHTOT 

6.0000000 

ACC  COMPENSATION  INITIALIZATION 


OT 

.100000000- 

-01  SEC 

FRNTSW 

1.0000000 

OUTSII 

0 

OFILE 

6 

SPARE 

.0 

MOD POT 

6.0090000 

PRNTDT 

6.0000000 

DELI 

.0 

GM  CM*>**2 

X 

Y 

z 

DIAS 

390.00000 

890.00000 

690.00000 

KII 

.60000000 

.60000000 

.60000000 

NTC 

.70000000 

GM  CM 

I XX 

S. 0000000 

GM  CH“»2 

OADX 

1.0000000 

.0 

.0 

.0 

.0 

1.0000000 

.0 

-1.0000000 

.0 

GABY 

.0 

1.0000000 

.0 

1.0000000 

.0 

.0 

.0 

.0 

•1.0000000 

GAO  Z 

.0 

.0 

1.0000000 

MICRO  G 
MICRO  G/G««2 
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tmt*  > 

i 

i 

1.0000000 

.0 

.0 

1 

.0 

1.0000000 

.0 

QiilS 

1.0000000 

.0 

.0 

.0 

1.0000000 

.0 

.0 

.0 

1.0000000 

SFMO 

10.000000 

10.000000 

10.000000 

PPM 

5FN1 

.0 

.0 

.0 

PPM  /G 

SFF3 

10.000000 

10.000000 

10.000000 

PPM 

SFFX 

.0 

.0 

.0 

PPM  /G 

RX 

1.0100000 

.0 

.0 

FT 

RY 

.6*000000 

.19000000 

.0 

FT 

RZ 

.91000000 

.0 

.19000000 

FT 

6YR  COMPENSATION  INITIALIZATION 

OT 

. 100000000-01 

SEC 

rvuTsu 

1.0000300 

C'JTON 

0 

CFILE 

6 

traroT 

6.OOOCOOO 

f.tntot 

6.0000000 

X 

Y 

2 

C l AS 

-. 34000000 

- . 3*000000 

-.34000000 

DEG/HR 

Sl'PO 

-30.000000 

-30,000000 

-30.000000 

PPM 

SFMO 

-*4.000000 

-44.000000 

-44.000000 

PPM 

C33X 

1.0000000 

.0 

.0 

.0 

.0 

1.0000000 

.0 

-1.0000000 

.0 

CCCY 

.0 

1.0000000 

.0 

1.0000000 

.0 

.0 

.0 

.0 

-1.0000000 

croz 

.0 

.0 

1.0000000 

1.0000000 

.0 

.0 

.0 

1.0090000 

.0 

CM!  3 

1.C30C000 

.0 

.0 

.0 

1.0000090 

,0 

.0 

.0 

1.0000000 

KI 

1.1600000 

1.1600300 

1.1600000 

OEG/tIR/G 

KO 

-.370000000-01 

- . 370000000-01 

-.370000000-01 

CEG/II3/G 

KS 

- .66000000 

-.66000000 

-.66000000 

DEO/MR/G 

xrr 

.0 

.0 

.0 

occ/im/GMfj, 

KX3 

.603000009-01 

,600000000-01 

.600000000-01 

OEG/MfVG^O. 

K03 

-.150000000-01 

-.150000000-01 

-.150000000-01  DEG/Mr?/r.«#2, 

KOS 

.0 

.0 

.0 

DrO/1!t.V6**C, 

KIO 

.0 

.0 

.0 

DEG/HR/G«<, 

M 

151000.00 

CM  CM<m>C/SCC 

OCU 

14.000300 

CM  CM*-2 

IXX 

£26.00000 

GH  CH**0 

3FM1 

.0 

.0 

.0 

PPM  /RAD/SEC 

SFP1 

.0 

.0 

.0 

PFM  /fiAD/SEC 
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910s  0.69636424  -0.70710673  -0,12276700 

0.645C64ES  0.54167522  0.53096131 

-0,31459206  -0.45451943  0.63333299 

ALGORITHM  INITIALIZATION 


or 

.10003000 

PRfiTSM 

1.0000000 

CUTEH 

0 

OFILE 

4 

CORDCN 

1.0000000 

IIODPOT 

6.0000000 

PRtlTDT 

6.0000000 

QUAT 

.0 

.0 

ORDER 

3 

D7N-M 

4.0000000 

LA7EFR 

.0 

CEO 

ICK'E”R 

.0 

DEG 

tlAlliFR 

.0 

DEG 

PITERR 

.0 

OEG 

RQL-IIR 

.0 

DEG 

YANERR 

.0 

DEG 

A TRANSPOSE 


r j 
§j  i 

b' 


. 76604444 
.45451940 
.45451946 


.0 

.70710670 

-.70710676 


NAVIGATION  INITIALIZATION 


.64270761 

.54167522 

.54167522 


OT 

PRNT5W 

OUTSU 

oriLE 

SPARE 

PRNTOT 

CVOKSEC-1) 

CV02I5EC-2) 


.1COOOOOO  s 

1.0000000 

.0 

6 

.0 

6.0000000 

.600000000-01 

.162000030-02 


CVO 31  SEC-3)  .162000000-04 


INITIAL  VEHICLE  POSITION 


NIPT) 

ALTSBRtFT) 

V(  FT/SEC I 
VCRR1  FT/SEC) 
LaTIDEGSI 
LATCFINOEOS) 
LON  CESS) 
LO.'iEKfHOEGS) 


2000.0000 

.0 

1000.0000 

.0 

40.000000 

.0 

.0 

.0 


1000.0000 

.0 


100.00000 

.0 


EVALUATION 

INITIALIZATION 

OT 

.10000000 

PRNY5N 

1.0000000 

OUTSU 

Of  ILE 

6 

PRNTOT 

1.0000000 
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3.2  MODULE 


This  sample  page  printed  out  every  PRNTDT  or  MODPDT  interval 
display  the  values  of  significant  variables  at  the  printing  time  for 
each  of  the  modules. 


SHIS  PASS  IS  BEST  QUAitlY  HUC2IGAHL3 
UWi  WFY  PUBLISHED  TO  DDC 


6/23/74  SAMPLE  BUN  WITH  0EFAULT  DATA 
««  S£Q  **  TIH£s  .10000000 


*»  TRJ  **  ADI FT/SEC2  J 
W3I RAO/SEC) 
LATIDECS) 
ID!  II  DECS) 
ALF(OEGS) 
ALT! FT) 

VEU  FT/SEC) 
O VI  FT/SEC) 


5.0414927 

.532607050-04 

40.000274 

.356901650-03 

44.999771 

2010.0000 

1000.0000 

-.117041500-01 


.10130310 

.107115590-03 


1000.0000 

.130559920-01 


31.456426 

.707570200-04 


100.00000 

3.1944275 


QOI  .69635999 
-.70711293 
-.12277654 


.64506335 
. E4167304 
.53096530 


-.31460459 

-.45451251 

.03333206 


ENV  «*  ADI FT/SEC2 I 
W3I RAO/SEC) 
AUDI FT/SEC2I 
l!23l  PA0/5EC  J 
WEOOOTIR/S2) 


5.5614927 

.536607C50-04 

5.5614927 

-530607050-04 

.470246370-00 


.10130310 

.107115590-03 

.10130310 

.107115590-03 

>.255999640-00 


31.456426 

.707570200-04 

31.456426 

.707570200-04 

.674477150-00 


o GYR  DTHETAI RADS)  .545604470-05  .105015430-04  .769904950-05 


M ACC  *•«  0V| FT/SEC > 


.55270749 


.150564690-01  3.1425001 


*#  ALT  *4  ALT! FT) 


2010.0000 


W BOS  4*  0VIFT/SEC) 
DTHETAI RA0S) 


.55270749  .150564690-01  3.1425001 

.545604470-05  .105015430-04  .749904950-05 


t . 


44  CAC  44  0V  IFT/5EC) 
M CGY  44  D THETA  (RAD) 


.55565060 


.179224120-01  3.1453971 


.537233000-05  .107301130-04  ,707076770-05 


44  AlO  4«  0VWFT/3EC) 


2.4302064 


-.119321910-01  2.0633971 


OCH  -.31459953 
.69636720 
.64506334 


-.45451760 

-.70710961 

.54167303 


.03333116 

-.12270261 

.53096532 


44  UM  44  HI  FT  I 

VI  FT/SEC) 
LATIDECS) 
LCtllUEGS) 
PITCH! DECS  I 
ROLUOEGSl 
HEA01H0I0GGS) 


2010.0000 
999.99904 
40.000274 
.354901000-03 
9. 9999909 
-.016306260-06 
45.000000 


999.99954 


99.999471 


A TkA!S3I>0$E 

.76604134  . 4 77174630-05  . 64279120 

'.4545246V  .707)0670  .54)67007 

'.45451949  -.70710670  .54167521 

OTIH 

.17164016  -.140307030-07  .95480776 

. 96400776  -.400435900-05  -.17364816 

.394615710-05  1.0000000  -.601566670-06 
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3.3  EVALUATION 


:•  : , 1 Two  sample  pages  display  the  50  module  cycle  confutations  of 

1 trajectory  parameters  and  navigational  errors. 
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